From 48c499be8549ed49cdaf3ec23f9df5f69c994530 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 9 Mar 2022 13:22:07 +0100 Subject: [PATCH 01/33] USD -> SDF: Added cmd line tool Signed-off-by: ahcorde --- usd/src/CMakeLists.txt | 1 + usd/src/cmd/CMakeLists.txt | 11 ++++ usd/src/cmd/usd2sdf.cc | 83 ++++++++++++++++++++++++++++++ usd/src/usd_parser/usd2sdf_TEST.cc | 74 ++++++++++++++++++++++++++ 4 files changed, 169 insertions(+) create mode 100644 usd/src/cmd/usd2sdf.cc create mode 100644 usd/src/usd_parser/usd2sdf_TEST.cc diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 0531a17a9..8f31fade4 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -31,6 +31,7 @@ target_link_libraries(${usd_target} set(gtest_sources sdf_parser/sdf2usd_TEST.cc + usd_parser/usd2sdf_TEST.cc sdf_parser/Geometry_Sdf2Usd_TEST.cc sdf_parser/Joint_Sdf2Usd_TEST.cc sdf_parser/Light_Sdf2Usd_TEST.cc diff --git a/usd/src/cmd/CMakeLists.txt b/usd/src/cmd/CMakeLists.txt index 46b6ca8a3..2f480353a 100644 --- a/usd/src/cmd/CMakeLists.txt +++ b/usd/src/cmd/CMakeLists.txt @@ -9,9 +9,20 @@ if(TARGET ${usd_target}) ${usd_target} ) + add_executable(usd2sdf + usd2sdf.cc + ) + + target_link_libraries(usd2sdf + PUBLIC + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} + ${usd_target} + ) + install( TARGETS sdf2usd + usd2sdf DESTINATION ${BIN_INSTALL_DIR} ) diff --git a/usd/src/cmd/usd2sdf.cc b/usd/src/cmd/usd2sdf.cc new file mode 100644 index 000000000..259fe07f7 --- /dev/null +++ b/usd/src/cmd/usd2sdf.cc @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include "sdf/sdf.hh" + +////////////////////////////////////////////////// +/// \brief Enumeration of available commands +enum class Command +{ + kNone, +}; + +////////////////////////////////////////////////// +/// \brief Structure to hold all filenames +struct Options +{ + /// \brief Command to execute + Command command{Command::kNone}; + + /// \brief input filename + std::string inputFilename{"input.usd"}; + + /// \brief output filename + std::string outputFilename{"output.sdf"}; +}; + +void runCommand(const Options &/*_opt*/) +{ + // TODO(ahcorde): Call here the USD to SDF conversor code +} + +void addFlags(CLI::App &_app) +{ + auto opt = std::make_shared(); + + _app.add_option("input", + opt->inputFilename, + "Input filename. Defaults to input.usd unless otherwise specified."); + + _app.add_option("output", + opt->outputFilename, + "Output filename. Defaults to output.sdf unless otherwise specified."); + + _app.callback([&_app, opt](){ + runCommand(*opt); + }); +} + +////////////////////////////////////////////////// +int main(int argc, char** argv) +{ + CLI::App app{"USD to SDF converter"}; + + app.set_help_all_flag("--help-all", "Show all help"); + + app.add_flag_callback("--version", [](){ + std::cout << SDF_VERSION_FULL << std::endl; + throw CLI::Success(); + }); + + addFlags(app); + CLI11_PARSE(app, argc, argv); + + return 0; +} diff --git a/usd/src/usd_parser/usd2sdf_TEST.cc b/usd/src/usd_parser/usd2sdf_TEST.cc new file mode 100644 index 000000000..15a68bca0 --- /dev/null +++ b/usd/src/usd_parser/usd2sdf_TEST.cc @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include + +#include + +#include +#include +#include + +#include "test_config.h" +#include "test_utils.hh" + +#ifdef _WIN32 + #define popen _popen + #define pclose _pclose +#endif + +static std::string usd2sdfCommand() +{ + return ignition::common::joinPaths(std::string(PROJECT_BINARY_DIR), "bin", + "usd2sdf"); +} + +///////////////////////////////////////////////// +std::string custom_exec_str(std::string _cmd) +{ + _cmd += " 2>&1"; + FILE *pipe = popen(_cmd.c_str(), "r"); + + if (!pipe) + return "ERROR"; + + char buffer[128]; + std::string result = ""; + + while (!feof(pipe)) + { + if (fgets(buffer, 128, pipe) != NULL) + result += buffer; + } + + pclose(pipe); + return result; +} + +///////////////////////////////////////////////// +TEST(version_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + // Check a good SDF file + { + std::string output = + custom_exec_str(usd2sdfCommand() + " --version"); + + EXPECT_EQ(output, std::string(SDF_VERSION_FULL) + "\n"); + + // TODO(ahcorde): Check the contents of outputUsdFilePath when the parser + // is implemented + } +} From 3a9b3493ea7f094b93ddf10fb01e3a9fb4c80875 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 9 Mar 2022 13:48:56 +0100 Subject: [PATCH 02/33] USD -> SDF: Parse Physics and generated SDF file Signed-off-by: ahcorde --- usd/include/sdf/usd/usd_parser/ParseUSD.hh | 47 ++++++ usd/src/CMakeLists.txt | 4 + usd/src/cmd/usd2sdf.cc | 5 +- usd/src/usd_parser/ParseUSD.cc | 43 ++++++ usd/src/usd_parser/Physics.cc | 49 ++++++ usd/src/usd_parser/Physics.hh | 49 ++++++ usd/src/usd_parser/USD.cc | 71 +++++++++ usd/src/usd_parser/USD.hh | 47 ++++++ usd/src/usd_parser/USD2SDF.cc | 145 ++++++++++++++++++ usd/src/usd_parser/USD2SDF.hh | 77 ++++++++++ usd/src/usd_parser/usd2sdf_TEST.cc | 22 +++ .../usd_parser/usd_model/WorldInterface.hh | 61 ++++++++ 12 files changed, 618 insertions(+), 2 deletions(-) create mode 100644 usd/include/sdf/usd/usd_parser/ParseUSD.hh create mode 100644 usd/src/usd_parser/ParseUSD.cc create mode 100644 usd/src/usd_parser/Physics.cc create mode 100644 usd/src/usd_parser/Physics.hh create mode 100644 usd/src/usd_parser/USD.cc create mode 100644 usd/src/usd_parser/USD.hh create mode 100644 usd/src/usd_parser/USD2SDF.cc create mode 100644 usd/src/usd_parser/USD2SDF.hh create mode 100644 usd/src/usd_parser/usd_model/WorldInterface.hh diff --git a/usd/include/sdf/usd/usd_parser/ParseUSD.hh b/usd/include/sdf/usd/usd_parser/ParseUSD.hh new file mode 100644 index 000000000..c0ed0f2f7 --- /dev/null +++ b/usd/include/sdf/usd/usd_parser/ParseUSD.hh @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SDF_USD_USD_PARSER_PARSE_USD_HH +#define SDF_USD_USD_PARSER_PARSE_USD_HH + +#include + +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief It parses a USD file and it converted to SDF + /// \param[in] _inputFilename Path of the USD file to parse + /// \param[in] _outputFilename_sdf Path where the SDF file will be located + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when parsing to its SDF representation. + UsdErrors IGNITION_SDFORMAT_USD_VISIBLE parseUSDFile( + const std::string &_inputFilename, + const std::string &_outputFilename_sdf); + } + } +} +#endif diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 8f31fade4..7d9df2472 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -10,6 +10,10 @@ set(sources sdf_parser/Sensor.cc sdf_parser/Visual.cc sdf_parser/World.cc + usd_parser/ParseUSD.cc + usd_parser/Physics.cc + usd_parser/USD.cc + usd_parser/USD2SDF.cc usd_parser/USDData.cc usd_parser/USDMaterial.cc usd_parser/USDStage.cc diff --git a/usd/src/cmd/usd2sdf.cc b/usd/src/cmd/usd2sdf.cc index 259fe07f7..d4b198ec3 100644 --- a/usd/src/cmd/usd2sdf.cc +++ b/usd/src/cmd/usd2sdf.cc @@ -19,6 +19,7 @@ #include +#include "sdf/usd/usd_parser/ParseUSD.hh" #include "sdf/sdf.hh" ////////////////////////////////////////////////// @@ -42,9 +43,9 @@ struct Options std::string outputFilename{"output.sdf"}; }; -void runCommand(const Options &/*_opt*/) +void runCommand(const Options &_opt) { - // TODO(ahcorde): Call here the USD to SDF conversor code + sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename); } void addFlags(CLI::App &_app) diff --git a/usd/src/usd_parser/ParseUSD.cc b/usd/src/usd_parser/ParseUSD.cc new file mode 100644 index 000000000..aa4f4919c --- /dev/null +++ b/usd/src/usd_parser/ParseUSD.cc @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/usd/usd_parser/ParseUSD.hh" +#include "USD2SDF.hh" + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { +namespace usd +{ + UsdErrors parseUSDFile( + const std::string &_inputFilename, const std::string &_outputFilename) + { + UsdErrors errors; + USD2SDF usd2sdf; + auto doc = tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE); + auto readErrors = usd2sdf.Read(_inputFilename, &doc); + if (!readErrors.empty()) + { + errors.insert(errors.end(), readErrors.begin(), readErrors.end()); + return errors; + } + doc.SaveFile(_outputFilename.c_str()); + return errors; + } +} +} +} diff --git a/usd/src/usd_parser/Physics.cc b/usd/src/usd_parser/Physics.cc new file mode 100644 index 000000000..73245e757 --- /dev/null +++ b/usd/src/usd_parser/Physics.cc @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "Physics.hh" + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Console.hh" + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { +namespace usd +{ + void ParsePhysicsScene( + const pxr::UsdPrim &_prim, + std::shared_ptr &_world, + double _metersPerUnit) + { + auto variant_physics_scene = pxr::UsdPhysicsScene(_prim); + pxr::GfVec3f gravity; + variant_physics_scene.GetGravityDirectionAttr().Get(&gravity); + _world->gravity[0] = gravity[0]; + _world->gravity[1] = gravity[1]; + _world->gravity[2] = gravity[2]; + variant_physics_scene.GetGravityMagnitudeAttr().Get(&_world->magnitude); + _world->magnitude *= _metersPerUnit; + } +} +} +} diff --git a/usd/src/usd_parser/Physics.hh b/usd/src/usd_parser/Physics.hh new file mode 100644 index 000000000..e5ff1f907 --- /dev/null +++ b/usd/src/usd_parser/Physics.hh @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef USD_PARSER_PHYSYCS_HH +#define USD_PARSER_PHYSYCS_HH + +#include "usd_model/WorldInterface.hh" + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief It parses the physics attributes of the USD file + /// \param[in] _prim Prim to extract the physics attributes + /// \param[out] _world World interface where the data is placed + /// \param[in] _metersPerUnit meter per unit in the USD + void ParsePhysicsScene( + const pxr::UsdPrim &_prim, + std::shared_ptr &_world, + double _metersPerUnit); + } + } +} + +#endif diff --git a/usd/src/usd_parser/USD.cc b/usd/src/usd_parser/USD.cc new file mode 100644 index 000000000..c5f585a24 --- /dev/null +++ b/usd/src/usd_parser/USD.cc @@ -0,0 +1,71 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include "USD.hh" + +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/usd/usd_parser/USDStage.hh" +#include "Physics.hh" + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +namespace sdf +{ +inline namespace SDF_VERSION_NAMESPACE { +namespace usd +{ + UsdErrors parseUSDWorld( + const std::string &_inputFilename, + std::shared_ptr &_world) + { + UsdErrors errors; + USDData usdData(_inputFilename); + usdData.Init(); + usdData.ParseMaterials(); + + auto referencee = pxr::UsdStage::Open(_inputFilename); + if (!referencee) + { + errors.emplace_back(UsdError( + UsdErrorCode::INVALID_USD_FILE, + "Unable to open [" + _inputFilename + "]")); + return errors; + } + auto range = pxr::UsdPrimRange::Stage(referencee); + + _world->_worldName = referencee->GetDefaultPrim().GetName().GetText(); + + for (auto const &prim : range) + { + if (prim.IsA()) + { + std::pair> data = + usdData.FindStage(prim.GetPath().GetName()); + + ParsePhysicsScene(prim, _world, data.second->MetersPerUnit()); + continue; + } + } + return errors; + } +} +} +} diff --git a/usd/src/usd_parser/USD.hh b/usd/src/usd_parser/USD.hh new file mode 100644 index 000000000..38171d912 --- /dev/null +++ b/usd/src/usd_parser/USD.hh @@ -0,0 +1,47 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef USD_PARSER_USD_HH +#define USD_PARSER_USD_HH + +#include + +#include "sdf/sdf_config.h" +#include "sdf/usd/UsdError.hh" + +#include "usd_model/WorldInterface.hh" + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief it parses the USD file + /// \param[in] _inputFilename Path where the USD is located + /// \param[out] _world World interface where all USD data is placed + /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError + /// includes an error code and message. An empty vector indicates no error + /// occurred when parsing to its SDF representation. + UsdErrors parseUSDWorld( + const std::string &_inputFilename, + std::shared_ptr &_world); + } + } +} +#endif diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc new file mode 100644 index 000000000..10bdf891b --- /dev/null +++ b/usd/src/usd_parser/USD2SDF.cc @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "USD2SDF.hh" + +#include "usd_model/WorldInterface.hh" + +#include "USD.hh" + +namespace sdf { +namespace usd { +///////////////////////////////////////////////// +USD2SDF::USD2SDF() +{ +} + +///////////////////////////////////////////////// +USD2SDF::~USD2SDF() +{ +} + +///////////////////////////////////////////////// +UsdErrors USD2SDF::Read(const std::string &_filename, + tinyxml2::XMLDocument* _sdfXmlOut) +{ + UsdErrors errors; + + std::shared_ptr worldInterface = + std::make_shared(); + + auto errorsParseUSD = + parseUSDWorld(_filename, worldInterface); + if (!errorsParseUSD.empty()) + { + errors.emplace_back(UsdError( + UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Error parsing the usd file")); + return errors; + } + + tinyxml2::XMLElement *sdf = nullptr; + tinyxml2::XMLElement *world = nullptr; + + sdf = _sdfXmlOut->NewElement("sdf"); + sdf->SetAttribute("version", "1.7"); + + world = _sdfXmlOut->NewElement("world"); + std::string worldName = worldInterface->_worldName; + if (worldName.empty()) + { + worldName = "world_name"; + } + world->SetAttribute("name", (worldName + "_world").c_str()); + + AddKeyValue(world, "gravity", Vector32Str( + worldInterface->gravity * worldInterface->magnitude)); + + sdf->LinkEndChild(world); + _sdfXmlOut->LinkEndChild(sdf); + return errors; +} + +//////////////////////////////////////////////////////////////////////////////// +std::string USD2SDF::GetKeyValueAsString(tinyxml2::XMLElement* _elem) +{ + std::string valueStr; + if (_elem->Attribute("value")) + { + valueStr = _elem->Attribute("value"); + } + else if (_elem->FirstChild()) + { + // Check that this node is a XMLText + if (_elem->FirstChild()->ToText()) + { + valueStr = _elem->FirstChild()->Value(); + } + else + { + sdfwarn << "Attribute value string not set\n"; + } + } + return trim(valueStr); +} + +//////////////////////////////////////////////////////////////////////////////// +void USD2SDF::AddKeyValue( + tinyxml2::XMLElement *_elem, + const std::string &_key, + const std::string &_value) +{ + tinyxml2::XMLElement *childElem = _elem->FirstChildElement(_key.c_str()); + if (childElem) + { + std::string oldValue = GetKeyValueAsString(childElem); + if (oldValue != _value) + { + sdferr << "multiple inconsistent <" << _key + << "> exists due to fixed joint reduction" + << " overwriting previous value [" << oldValue + << "] with [" << _value << "].\n"; + } + else + { + sdferr << "multiple consistent <" << _key + << "> exists with [" << _value + << "] due to fixed joint reduction.\n"; + } + _elem->DeleteChild(childElem); // remove old _elem + } + + auto *doc = _elem->GetDocument(); + tinyxml2::XMLElement *ekey = doc->NewElement(_key.c_str()); + tinyxml2::XMLText *textEkey = doc->NewText(_value.c_str()); + ekey->LinkEndChild(textEkey); + _elem->LinkEndChild(ekey); +} + +///////////////////////////////////////////////// +std::string USD2SDF::Vector32Str(const ignition::math::Vector3d _vector) +{ + std::stringstream ss; + ss << _vector.X(); + ss << " "; + ss << _vector.Y(); + ss << " "; + ss << _vector.Z(); + return ss.str(); +} +} +} diff --git a/usd/src/usd_parser/USD2SDF.hh b/usd/src/usd_parser/USD2SDF.hh new file mode 100644 index 000000000..52f64c322 --- /dev/null +++ b/usd/src/usd_parser/USD2SDF.hh @@ -0,0 +1,77 @@ +/* + * Copyright 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SDF_USD_USD_PARSER_USD2SDF_HH +#define SDF_USD_USD_PARSER_USD2SDF_HH + +#include + +#include + +#include + +#include + +#include "sdf/usd/UsdError.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + /// \brief USD to SDF converter + /// This class helps to generate the SDF file + namespace usd + { + class USD2SDF + { + /// \brief constructor + public: USD2SDF(); + + /// \brief destructor + public: ~USD2SDF(); + + /// \brief convert USD file to sdf xml document + /// \param[in] _filename string containing filename of the urdf model. + /// \param[inout] _sdfXmlDoc Document to populate with the sdf model. + public: UsdErrors Read( + const std::string &_filename, + tinyxml2::XMLDocument* _sdfXmlOut); + + /// \brief get value from pair and return it as string + /// \param[in] _elem pointer to xml element + /// return a string with the key + private: std::string GetKeyValueAsString(tinyxml2::XMLElement* _elem); + + /// \brief append key value pair to the end of the xml element + /// \param[in] _elem pointer to xml element + /// \param[in] _key string containing key to add to xml element + /// \param[in] _value string containing value for the key added + private: void AddKeyValue( + tinyxml2::XMLElement *_elem, + const std::string &_key, + const std::string &_value); + + /// \brief convert Vector3 to string + /// \param[in] _vector a ignition::math::Vector3d + /// \return a string + private: std::string Vector32Str(const ignition::math::Vector3d _vector); + }; + } + } +} + +#endif diff --git a/usd/src/usd_parser/usd2sdf_TEST.cc b/usd/src/usd_parser/usd2sdf_TEST.cc index 15a68bca0..9c624dc4c 100644 --- a/usd/src/usd_parser/usd2sdf_TEST.cc +++ b/usd/src/usd_parser/usd2sdf_TEST.cc @@ -72,3 +72,25 @@ TEST(version_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) // is implemented } } + +///////////////////////////////////////////////// +TEST(check_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + auto tmpDir = ignition::common::tempDirectoryPath(); + auto tmp = ignition::common::createTempDirectory("usd", tmpDir); + // Check a good SDF file + { + std::string path = sdf::testing::TestFile("usd", "upAxisZ.usda"); + const auto outputUsdFilePath = + ignition::common::joinPaths(tmp, "upAxisZ.sdf"); + EXPECT_FALSE(ignition::common::isFile(outputUsdFilePath)); + std::string output = + custom_exec_str(usd2sdfCommand() + " " + path + " " + outputUsdFilePath); + + // make sure that a shapes.usd file was generated + EXPECT_TRUE(ignition::common::isFile(outputUsdFilePath)) << output; + + // TODO(anyone): Check the contents of outputUsdFilePath when the parser + // is implemented + } +} diff --git a/usd/src/usd_parser/usd_model/WorldInterface.hh b/usd/src/usd_parser/usd_model/WorldInterface.hh new file mode 100644 index 000000000..3a7d09141 --- /dev/null +++ b/usd/src/usd_parser/usd_model/WorldInterface.hh @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef USD_PARSER_USD_MODEL_WORLD_INTERFACE_HH_ +#define USD_PARSER_USD_MODEL_WORLD_INTERFACE_HH_ + +#include +#include +#include + +#include + +#include +#include + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief This class store data about the world + class WorldInterface { + public: + /// \brief World name + std::string _worldName; + + /// \brief Magnitude of the gravity + float magnitude; + + /// \brief Gravity (X, Y, Z) + ignition::math::Vector3d gravity; + + friend std::ostream& operator<<( + std::ostream& os, const WorldInterface& _world) + { + os << "World name: " << _world._worldName; + os << "Gravity: " << _world.gravity * _world.magnitude << "\n"; + return os; + } + }; + } + } +} + +#endif From 576f9bd6fdfdd972c9d61f15182fa65201d003b2 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 9 Mar 2022 14:05:05 +0100 Subject: [PATCH 03/33] usd -> sdf: Read transforms Signed-off-by: ahcorde --- .../sdf/usd/usd_parser/USDTransforms.hh | 120 ++++++++ usd/src/CMakeLists.txt | 2 + usd/src/usd_parser/USDTransforms.cc | 261 ++++++++++++++++++ usd/src/usd_parser/USDTransforms_TEST.cc | 194 +++++++++++++ 4 files changed, 577 insertions(+) create mode 100644 usd/include/sdf/usd/usd_parser/USDTransforms.hh create mode 100644 usd/src/usd_parser/USDTransforms.cc create mode 100644 usd/src/usd_parser/USDTransforms_TEST.cc diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh new file mode 100644 index 000000000..22764d9d8 --- /dev/null +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -0,0 +1,120 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef SDF_USD_USD_PARSER_UTILS_HH_ +#define SDF_USD_USD_PARSER_UTILS_HH_ + +#include +#include + +#include +#include + +// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/sdf_config.h" +#include "sdf/system_util.hh" +#include "sdf/usd/Export.hh" +#include "sdf/usd/UsdError.hh" + +#include "USDData.hh" + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief This class stores the transforms of a schema + /// This might contains scale, translate or rotation operations + /// The booleans are used to check if there is a transform defined + /// in the schema + /// Rotation is splitted in a vector because this might be defined + /// as a rotation of 3 angles (ZYX, XYZ, etc). + class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms + { + public: + /// \brief Scale of the schema + ignition::math::Vector3d scale{1, 1, 1}; + + /// \brief Rotation of the schema + std::vector q; + + /// \brief Translatio of the schema + ignition::math::Vector3d translate{0, 0, 0}; + + /// \brief True if there is a rotation ZYX definedor false otherwise + bool isRotationZYX = false; + + /// \brief True if there is a rotation (as a quaterion) defined + /// or false otherwise + bool isRotation = false; + + /// \brief True if there is a translation defined or false otherwise + bool isTranslate = false; + }; + + /// \brief This function will parse all the parents transforms of a prim + /// This will stop when the name of the parent is the same as _schemaToStop + /// \param[in] _prim Initial prim to read the transform + /// \param[in] _usdData USDData structure to get info about the prim, for + /// example: metersperunit + /// \param[out] _tfs A vector with all the transforms + /// \param[out] _scale The scale of the prims + /// \param[in] _schemaToStop Name of the prim where the loop will stop + /// reading transforms + void IGNITION_SDFORMAT_USD_VISIBLE GetAllTransforms( + const pxr::UsdPrim &_prim, + USDData &_usdData, + std::vector &_tfs, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop); + + /// \brief This function get the transform from a prim to the specified + /// schemaToStop variable + /// This will stop when the name of the parent is the same as _schemaToStop + /// \param[in] _prim Initial prim to read the transform + /// \param[in] _usdData USDData structure to get info about the prim, for + /// example: metersperunit + /// \param[out] _pose Pose of the prim + /// \param[out] _scale The scale of the prim + /// \param[in] _schemaToStop Name of the prim where the loop will stop + /// reading transforms + void IGNITION_SDFORMAT_USD_VISIBLE GetTransform( + const pxr::UsdPrim &_prim, + USDData &_usdData, + ignition::math::Pose3d &_pose, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop); + + /// \brief Read the usd prim transforms. Scale, rotation or transform might + /// be defined as float or doubles + /// \param[in] _prim Prim where the transforms are read + UDSTransforms IGNITION_SDFORMAT_USD_VISIBLE ParseUSDTransform( + const pxr::UsdPrim &_prim); +} +} +} +#endif diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 7d9df2472..bed753570 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -17,6 +17,7 @@ set(sources usd_parser/USDData.cc usd_parser/USDMaterial.cc usd_parser/USDStage.cc + usd_parser/USDTransforms.cc ) ign_add_component(usd SOURCES ${sources} GET_TARGET_NAME usd_target) @@ -46,6 +47,7 @@ set(gtest_sources sdf_parser/World_Sdf2Usd_TEST.cc usd_parser/USDData_TEST.cc usd_parser/USDStage_TEST.cc + usd_parser/USDTransforms_TEST.cc Conversions_TEST.cc UsdError_TEST.cc UsdUtils_TEST.cc diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc new file mode 100644 index 000000000..ed6e6f0ea --- /dev/null +++ b/usd/src/usd_parser/USDTransforms.cc @@ -0,0 +1,261 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "sdf/usd/usd_parser/USDTransforms.hh" +#include "sdf/usd/usd_parser/USDData.hh" + +#include +#include + +namespace sdf +{ + // Inline bracke to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + + void GetAllTransforms( + const pxr::UsdPrim &_prim, + USDData &_usdData, + std::vector &_tfs, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop) + { + pxr::UsdPrim parent = _prim; + double metersPerUnit = 1.0; + std::string upAxis = "Y"; + + // this assumes that there can only be one stage + auto stageData = _usdData.FindStage(parent.GetPath().GetName()); + if (stageData.second != nullptr) { + metersPerUnit = stageData.second->MetersPerUnit(); + upAxis = stageData.second->UpAxis(); + } + + while (parent) + { + if (pxr::TfStringify(parent.GetPath()) == _schemaToStop) + { + return; + } + + UDSTransforms t = ParseUSDTransform(parent); + + ignition::math::Pose3d pose; + _scale *= t.scale; + + pose.Pos() = t.translate * metersPerUnit; + // scaling is lost when we convert to pose, so we pre-scale the translation + // to make them match the scaled values. + if (!_tfs.empty()) { + auto& child = _tfs.back(); + child.Pos().Set( + child.Pos().X() * t.scale[0], + child.Pos().Y() * t.scale[1], + child.Pos().Z() * t.scale[2]); + } + + if (!t.isRotationZYX) + { + if (t.isRotation) + { + pose.Rot() = t.q[0]; + } + _tfs.push_back(pose); + } + else + { + ignition::math::Pose3d poseZ = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), t.q[2]); + ignition::math::Pose3d poseY = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), t.q[1]); + ignition::math::Pose3d poseX = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), t.q[0]); + + ignition::math::Pose3d poseT = ignition::math::Pose3d( + t.translate * metersPerUnit, + ignition::math::Quaterniond(1, 0, 0, 0)); + + _tfs.push_back(poseZ); + _tfs.push_back(poseY); + _tfs.push_back(poseX); + _tfs.push_back(poseT); + } + parent = parent.GetParent(); + } + + if (upAxis == "Y") + { + ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), + ignition::math::Quaterniond(IGN_PI_2, 0, 0)); + _tfs.push_back(poseUpAxis); + } + } + + void GetTransform( + const pxr::UsdPrim &_prim, + USDData &_usdData, + ignition::math::Pose3d &_pose, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop) + { + std::vector tfs; + GetAllTransforms(_prim, _usdData, tfs, _scale, _schemaToStop); + for (auto & rt : tfs) + { + _pose = rt * _pose; + } + } + + UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) + { + auto variant_geom = pxr::UsdGeomGprim(_prim); + auto transforms = variant_geom.GetXformOpOrderAttr(); + + pxr::GfVec3f scale(1, 1, 1); + pxr::GfVec3f translate(0, 0, 0); + pxr::GfQuatf rotationQuad(1, 0, 0, 0); + + UDSTransforms t; + + pxr::VtTokenArray xformOpOrder; + transforms.Get(&xformOpOrder); + for (auto & op: xformOpOrder) + { + if (op == "xformOp:scale") + { + auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:scale")); + if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f") + { + attribute.Get(&scale); + } + else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d") + { + pxr::GfVec3d scaleTmp(1, 1, 1); + attribute.Get(&scaleTmp); + scale[0] = scaleTmp[0]; + scale[1] = scaleTmp[1]; + scale[2] = scaleTmp[2]; + } + t.scale = ignition::math::Vector3d(scale[0], scale[1], scale[2]); + } + else if (op == "xformOp:rotateZYX" || op == "xformOp:rotateXYZ") + { + pxr::GfVec3f rotationEuler(0, 0, 0); + auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:rotateZYX")); + if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f") + { + attribute.Get(&rotationEuler); + } + else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d") + { + pxr::GfVec3d rotationEulerTmp(0, 0, 0); + attribute.Get(&rotationEulerTmp); + rotationEuler[0] = rotationEulerTmp[0]; + rotationEuler[1] = rotationEulerTmp[1]; + rotationEuler[2] = rotationEulerTmp[2]; + } + ignition::math::Quaterniond qX, qY, qZ; + ignition::math::Angle angleX(rotationEuler[0] * IGN_PI / 180.0); + ignition::math::Angle angleY(rotationEuler[1] * IGN_PI / 180.0); + ignition::math::Angle angleZ(rotationEuler[2] * IGN_PI / 180.0); + qX = ignition::math::Quaterniond(angleX.Normalized().Radian(), 0, 0); + qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0); + qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian()); + + t.q.push_back(qX); + t.q.push_back(qY); + t.q.push_back(qZ); + t.isRotationZYX = true; + t.isRotation = true; + } + else if (op == "xformOp:translate") + { + auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:translate")); + if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f") + { + attribute.Get(&translate); + } + else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d") + { + pxr::GfVec3d translateTmp(0, 0, 0); + attribute.Get(&translateTmp); + translate[0] = translateTmp[0]; + translate[1] = translateTmp[1]; + translate[2] = translateTmp[2]; + } + t.translate = ignition::math::Vector3d(translate[0], translate[1], translate[2]); + t.isTranslate = true; + } + else if (op == "xformOp:orient") + { + auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:orient")); + if (attribute.GetTypeName().GetCPPTypeName() == "GfQuatf") + { + attribute.Get(&rotationQuad); + } + else if (attribute.GetTypeName().GetCPPTypeName() == "GfQuatd") + { + pxr::GfQuatd rotationQuadTmp; + attribute.Get(&rotationQuadTmp); + rotationQuad.SetImaginary( + rotationQuadTmp.GetImaginary()[0], + rotationQuadTmp.GetImaginary()[1], + rotationQuadTmp.GetImaginary()[2]); + rotationQuad.SetReal(rotationQuadTmp.GetReal()); + } + ignition::math::Quaterniond q( + rotationQuad.GetReal(), + rotationQuad.GetImaginary()[0], + rotationQuad.GetImaginary()[1], + rotationQuad.GetImaginary()[2]); + t.q.push_back(q); + t.isRotation = true; + } + + if (op == "xformOp:transform") + { + // FIXME: Shear is lost (does sdformat support it?). + pxr::GfMatrix4d transform; + _prim.GetAttribute(pxr::TfToken("xformOp:transform")).Get(&transform); + const auto rot = transform.RemoveScaleShear(); + const auto scaleShear = transform * rot.GetInverse(); + + t.scale[0] = scaleShear[0][0]; + t.scale[1] = scaleShear[1][1]; + t.scale[2] = scaleShear[2][2]; + + const auto rotQuat = rot.ExtractRotationQuat(); + t.translate = ignition::math::Vector3d(transform[3][0], transform[3][1], transform[3][2]); + ignition::math::Quaterniond q( + rotQuat.GetReal(), + rotQuat.GetImaginary()[0], + rotQuat.GetImaginary()[1], + rotQuat.GetImaginary()[2] + ); + t.q.push_back(q); + t.isTranslate = true; + t.isRotation = true; + } + } + return t; + } +} +} +} diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc new file mode 100644 index 000000000..f45f133c0 --- /dev/null +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -0,0 +1,194 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include + +#include "test_config.h" +#include "test_utils.hh" + +// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/usd/usd_parser/USDTransforms.hh" + +void checkTransforms( + const std::string &_primName, + pxr::UsdStageRefPtr &_stage, + const ignition::math::Vector3d &_translation, + const std::vector &_rotation, + const ignition::math::Vector3d &_scale) +{ + pxr::UsdPrim prim = _stage->GetPrimAtPath(pxr::SdfPath(_primName)); + ASSERT_TRUE(prim); + + sdf::usd::UDSTransforms usdTransforms = + sdf::usd::ParseUSDTransform(prim); + + EXPECT_EQ(_translation, usdTransforms.translate); + EXPECT_EQ( + _translation != ignition::math::Vector3d(0, 0, 0), + usdTransforms.isTranslate); + EXPECT_EQ(_scale, usdTransforms.scale); + EXPECT_EQ(_rotation.size(), usdTransforms.q.size()); + for (unsigned int i = 0; i < _rotation.size(); ++i) + { + EXPECT_EQ(_rotation[i], usdTransforms.q[i]); + } + + EXPECT_EQ(!_rotation.empty(), usdTransforms.isRotation); +} + +///////////////////////////////////////////////// +TEST(Utils, GetTransform) +{ + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + checkTransforms( + "/shapes/ground_plane", + stage, + ignition::math::Vector3d(0, 0, -0.125), + { + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0) + }, + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/ground_plane/link/visual/geometry", + stage, + ignition::math::Vector3d(0, 0, 0), + {}, + ignition::math::Vector3d(100, 100, 0.25) + ); + + checkTransforms( + "/shapes/cylinder", + stage, + ignition::math::Vector3d(0, -1.5, 0.5), + { + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0) + }, + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/sphere", + stage, + ignition::math::Vector3d(0, 1.5, 0.5), + { + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0) + }, + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/capsule", + stage, + ignition::math::Vector3d(0, -3.0, 0.5), + { + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0) + }, + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/ellipsoid", + stage, + ignition::math::Vector3d(0, 3.0, 0.5), + { + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0) + }, + ignition::math::Vector3d(1, 1, 1) + ); + + checkTransforms( + "/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry", + stage, + ignition::math::Vector3d(0, 0, 0), + {}, + ignition::math::Vector3d(0.4, 0.6, 1) + ); + + checkTransforms( + "/shapes/sun", + stage, + ignition::math::Vector3d(0, 0, 10), + { + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0) + }, + ignition::math::Vector3d(1, 1, 1) + ); +} + +///////////////////////////////////////////////// +TEST(Utils, GetAllTransform) +{ + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(sdf::testing::TestFile("usd", "upAxisZ.usda")); + usdData.Init(); + + pxr::UsdPrim prim = stage->GetPrimAtPath( + pxr::SdfPath("/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")); + ASSERT_TRUE(prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/shapes"); + + EXPECT_EQ(ignition::math::Vector3d(0.4, 0.6, 1), scale); + EXPECT_EQ( + ignition::math::Pose3d( + ignition::math::Vector3d(0, 0.03, 0.005), + ignition::math::Quaterniond(1, 0, 0, 0)), + pose); +} + +///////////////////////////////////////////////// +/// Main +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From a8694047f062f67371b3567deea807724dc6c8ac Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 9 Mar 2022 19:50:37 +0100 Subject: [PATCH 04/33] Fixed var name Signed-off-by: ahcorde --- usd/src/usd_parser/USD.cc | 2 +- usd/src/usd_parser/USD2SDF.cc | 2 +- usd/src/usd_parser/usd_model/WorldInterface.hh | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/usd/src/usd_parser/USD.cc b/usd/src/usd_parser/USD.cc index c5f585a24..34e850448 100644 --- a/usd/src/usd_parser/USD.cc +++ b/usd/src/usd_parser/USD.cc @@ -51,7 +51,7 @@ namespace usd } auto range = pxr::UsdPrimRange::Stage(referencee); - _world->_worldName = referencee->GetDefaultPrim().GetName().GetText(); + _world->worldName = referencee->GetDefaultPrim().GetName().GetText(); for (auto const &prim : range) { diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc index 10bdf891b..1507b8024 100644 --- a/usd/src/usd_parser/USD2SDF.cc +++ b/usd/src/usd_parser/USD2SDF.cc @@ -59,7 +59,7 @@ UsdErrors USD2SDF::Read(const std::string &_filename, sdf->SetAttribute("version", "1.7"); world = _sdfXmlOut->NewElement("world"); - std::string worldName = worldInterface->_worldName; + std::string worldName = worldInterface->worldName; if (worldName.empty()) { worldName = "world_name"; diff --git a/usd/src/usd_parser/usd_model/WorldInterface.hh b/usd/src/usd_parser/usd_model/WorldInterface.hh index 3a7d09141..cb59b21e4 100644 --- a/usd/src/usd_parser/usd_model/WorldInterface.hh +++ b/usd/src/usd_parser/usd_model/WorldInterface.hh @@ -38,7 +38,7 @@ namespace sdf class WorldInterface { public: /// \brief World name - std::string _worldName; + std::string worldName; /// \brief Magnitude of the gravity float magnitude; @@ -49,7 +49,7 @@ namespace sdf friend std::ostream& operator<<( std::ostream& os, const WorldInterface& _world) { - os << "World name: " << _world._worldName; + os << "World name: " << _world.worldName; os << "Gravity: " << _world.gravity * _world.magnitude << "\n"; return os; } From 0d2f1aed6eaa3c759d7cc6459847e472ae32218e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 9 Mar 2022 23:44:02 +0100 Subject: [PATCH 05/33] Added tests Signed-off-by: ahcorde --- usd/src/CMakeLists.txt | 3 +- usd/src/usd_parser/USD.cc | 4 +- .../usd_parser/{Physics.cc => USDPhysics.cc} | 4 +- .../usd_parser/{Physics.hh => USDPhysics.hh} | 5 +- usd/src/usd_parser/USDPhysics_TEST.cc | 57 +++++++++++++++++++ 5 files changed, 66 insertions(+), 7 deletions(-) rename usd/src/usd_parser/{Physics.cc => USDPhysics.cc} (96%) rename usd/src/usd_parser/{Physics.hh => USDPhysics.hh} (92%) create mode 100644 usd/src/usd_parser/USDPhysics_TEST.cc diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 7d9df2472..59363bd9a 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -11,11 +11,11 @@ set(sources sdf_parser/Visual.cc sdf_parser/World.cc usd_parser/ParseUSD.cc - usd_parser/Physics.cc usd_parser/USD.cc usd_parser/USD2SDF.cc usd_parser/USDData.cc usd_parser/USDMaterial.cc + usd_parser/USDPhysics.cc usd_parser/USDStage.cc ) @@ -45,6 +45,7 @@ set(gtest_sources sdf_parser/Visual_Sdf2Usd_TEST.cc sdf_parser/World_Sdf2Usd_TEST.cc usd_parser/USDData_TEST.cc + usd_parser/USDPhysics_TEST.cc usd_parser/USDStage_TEST.cc Conversions_TEST.cc UsdError_TEST.cc diff --git a/usd/src/usd_parser/USD.cc b/usd/src/usd_parser/USD.cc index 34e850448..1f1160e39 100644 --- a/usd/src/usd_parser/USD.cc +++ b/usd/src/usd_parser/USD.cc @@ -18,7 +18,7 @@ #include "sdf/usd/usd_parser/USDData.hh" #include "sdf/usd/usd_parser/USDStage.hh" -#include "Physics.hh" +#include "USDPhysics.hh" #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED @@ -60,7 +60,7 @@ namespace usd std::pair> data = usdData.FindStage(prim.GetPath().GetName()); - ParsePhysicsScene(prim, _world, data.second->MetersPerUnit()); + ParseUSDPhysicsScene(prim, _world, data.second->MetersPerUnit()); continue; } } diff --git a/usd/src/usd_parser/Physics.cc b/usd/src/usd_parser/USDPhysics.cc similarity index 96% rename from usd/src/usd_parser/Physics.cc rename to usd/src/usd_parser/USDPhysics.cc index 73245e757..8bf8f627e 100644 --- a/usd/src/usd_parser/Physics.cc +++ b/usd/src/usd_parser/USDPhysics.cc @@ -15,7 +15,7 @@ * */ -#include "Physics.hh" +#include "USDPhysics.hh" #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED @@ -30,7 +30,7 @@ namespace sdf inline namespace SDF_VERSION_NAMESPACE { namespace usd { - void ParsePhysicsScene( + void ParseUSDPhysicsScene( const pxr::UsdPrim &_prim, std::shared_ptr &_world, double _metersPerUnit) diff --git a/usd/src/usd_parser/Physics.hh b/usd/src/usd_parser/USDPhysics.hh similarity index 92% rename from usd/src/usd_parser/Physics.hh rename to usd/src/usd_parser/USDPhysics.hh index e5ff1f907..1dc392a98 100644 --- a/usd/src/usd_parser/Physics.hh +++ b/usd/src/usd_parser/USDPhysics.hh @@ -25,7 +25,8 @@ #include #pragma pop_macro ("__DEPRECATED") -#include +#include "sdf/config.hh" +#include "sdf/usd/Export.hh" namespace sdf { @@ -38,7 +39,7 @@ namespace sdf /// \param[in] _prim Prim to extract the physics attributes /// \param[out] _world World interface where the data is placed /// \param[in] _metersPerUnit meter per unit in the USD - void ParsePhysicsScene( + void IGNITION_SDFORMAT_USD_VISIBLE ParseUSDPhysicsScene( const pxr::UsdPrim &_prim, std::shared_ptr &_world, double _metersPerUnit); diff --git a/usd/src/usd_parser/USDPhysics_TEST.cc b/usd/src/usd_parser/USDPhysics_TEST.cc new file mode 100644 index 000000000..ef6085932 --- /dev/null +++ b/usd/src/usd_parser/USDPhysics_TEST.cc @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include + +// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// are reported using #pragma message so normal diagnostic flags cannot remove +// them. This workaround requires this block to be used whenever usd is +// included. +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include "test_config.h" +#include "test_utils.hh" + +#include "USDPhysics.hh" +#include "usd_model/WorldInterface.hh" + +///////////////////////////////////////////////// +TEST(USDLightsTest, DistanceLight) +{ + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + pxr::UsdPrim prim = stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics")); + + std::shared_ptr worldInterface = + std::make_shared(); + + double metersPerUnit = 1.0; + + sdf::usd::ParseUSDPhysicsScene( + prim, worldInterface, metersPerUnit); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface->gravity); + EXPECT_NEAR(9.8, worldInterface->magnitude, 0.0001); +} From d939ca45eb8fc955a1d8ff037fcaad87809dc23a Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Wed, 9 Mar 2022 19:27:33 -0700 Subject: [PATCH 06/33] review feedback Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- .../usd/usd_parser/{ParseUSD.hh => Parser.hh} | 21 +++++++++---------- usd/src/CMakeLists.txt | 2 +- usd/src/cmd/usd2sdf.cc | 2 +- usd/src/usd_parser/{ParseUSD.cc => Parser.cc} | 9 ++++---- usd/src/usd_parser/USD2SDF.cc | 2 ++ 5 files changed, 19 insertions(+), 17 deletions(-) rename usd/include/sdf/usd/usd_parser/{ParseUSD.hh => Parser.hh} (65%) rename usd/src/usd_parser/{ParseUSD.cc => Parser.cc} (81%) diff --git a/usd/include/sdf/usd/usd_parser/ParseUSD.hh b/usd/include/sdf/usd/usd_parser/Parser.hh similarity index 65% rename from usd/include/sdf/usd/usd_parser/ParseUSD.hh rename to usd/include/sdf/usd/usd_parser/Parser.hh index c0ed0f2f7..6f638cb99 100644 --- a/usd/include/sdf/usd/usd_parser/ParseUSD.hh +++ b/usd/include/sdf/usd/usd_parser/Parser.hh @@ -15,32 +15,31 @@ * */ -#ifndef SDF_USD_USD_PARSER_PARSE_USD_HH -#define SDF_USD_USD_PARSER_PARSE_USD_HH +#ifndef SDF_USD_USD_PARSER_PARSER_HH +#define SDF_USD_USD_PARSER_PARSER_HH #include -#include "sdf/sdf_config.h" -#include "sdf/system_util.hh" +#include "sdf/config.hh" #include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" namespace sdf { - // Inline bracke to help doxygen filtering. + // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // namespace usd { - /// \brief It parses a USD file and it converted to SDF - /// \param[in] _inputFilename Path of the USD file to parse - /// \param[in] _outputFilename_sdf Path where the SDF file will be located + /// \brief Parse a USD file and convert it to a SDF file + /// \param[in] _inputFilenameUsd Path of the USD file to parse + /// \param[in] _outputFilenameSdf Path where the SDF file will be located /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error - /// occurred when parsing to its SDF representation. + /// occurred when parsing the USD file to its SDF representation. UsdErrors IGNITION_SDFORMAT_USD_VISIBLE parseUSDFile( - const std::string &_inputFilename, - const std::string &_outputFilename_sdf); + const std::string &_inputFilenameUsd, + const std::string &_outputFilenameSdf); } } } diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 59363bd9a..e2c7a31d5 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -10,7 +10,7 @@ set(sources sdf_parser/Sensor.cc sdf_parser/Visual.cc sdf_parser/World.cc - usd_parser/ParseUSD.cc + usd_parser/Parser.cc usd_parser/USD.cc usd_parser/USD2SDF.cc usd_parser/USDData.cc diff --git a/usd/src/cmd/usd2sdf.cc b/usd/src/cmd/usd2sdf.cc index d4b198ec3..63968264b 100644 --- a/usd/src/cmd/usd2sdf.cc +++ b/usd/src/cmd/usd2sdf.cc @@ -19,7 +19,7 @@ #include -#include "sdf/usd/usd_parser/ParseUSD.hh" +#include "sdf/usd/usd_parser/Parser.hh" #include "sdf/sdf.hh" ////////////////////////////////////////////////// diff --git a/usd/src/usd_parser/ParseUSD.cc b/usd/src/usd_parser/Parser.cc similarity index 81% rename from usd/src/usd_parser/ParseUSD.cc rename to usd/src/usd_parser/Parser.cc index aa4f4919c..787a10ce8 100644 --- a/usd/src/usd_parser/ParseUSD.cc +++ b/usd/src/usd_parser/Parser.cc @@ -15,7 +15,7 @@ * */ -#include "sdf/usd/usd_parser/ParseUSD.hh" +#include "sdf/usd/usd_parser/Parser.hh" #include "USD2SDF.hh" namespace sdf @@ -24,18 +24,19 @@ inline namespace SDF_VERSION_NAMESPACE { namespace usd { UsdErrors parseUSDFile( - const std::string &_inputFilename, const std::string &_outputFilename) + const std::string &_inputFilenameUsd, + const std::string &_outputFilenameSdf) { UsdErrors errors; USD2SDF usd2sdf; auto doc = tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE); - auto readErrors = usd2sdf.Read(_inputFilename, &doc); + auto readErrors = usd2sdf.Read(_inputFilenameUsd, &doc); if (!readErrors.empty()) { errors.insert(errors.end(), readErrors.begin(), readErrors.end()); return errors; } - doc.SaveFile(_outputFilename.c_str()); + doc.SaveFile(_outputFilenameSdf.c_str()); return errors; } } diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc index 1507b8024..2de75cb8b 100644 --- a/usd/src/usd_parser/USD2SDF.cc +++ b/usd/src/usd_parser/USD2SDF.cc @@ -22,6 +22,7 @@ #include "USD.hh" namespace sdf { +inline namespace SDF_VERSION_NAMESPACE { namespace usd { ///////////////////////////////////////////////// USD2SDF::USD2SDF() @@ -143,3 +144,4 @@ std::string USD2SDF::Vector32Str(const ignition::math::Vector3d _vector) } } } +} From bffdc8f466cc7b92b12bc8da18f6b5362770b259 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Wed, 9 Mar 2022 21:10:18 -0700 Subject: [PATCH 07/33] more review feedback Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- usd/src/CMakeLists.txt | 2 +- usd/src/usd_parser/USD2SDF.cc | 2 +- usd/src/usd_parser/{USD.cc => USDWorld.cc} | 33 +++++++++++++--------- usd/src/usd_parser/{USD.hh => USDWorld.hh} | 16 +++++------ 4 files changed, 29 insertions(+), 24 deletions(-) rename usd/src/usd_parser/{USD.cc => USDWorld.cc} (70%) rename usd/src/usd_parser/{USD.hh => USDWorld.hh} (71%) diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index e2c7a31d5..dc0ab51b5 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -11,12 +11,12 @@ set(sources sdf_parser/Visual.cc sdf_parser/World.cc usd_parser/Parser.cc - usd_parser/USD.cc usd_parser/USD2SDF.cc usd_parser/USDData.cc usd_parser/USDMaterial.cc usd_parser/USDPhysics.cc usd_parser/USDStage.cc + usd_parser/USDWorld.cc ) ign_add_component(usd SOURCES ${sources} GET_TARGET_NAME usd_target) diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc index 2de75cb8b..488fbd802 100644 --- a/usd/src/usd_parser/USD2SDF.cc +++ b/usd/src/usd_parser/USD2SDF.cc @@ -19,7 +19,7 @@ #include "usd_model/WorldInterface.hh" -#include "USD.hh" +#include "USDWorld.hh" namespace sdf { inline namespace SDF_VERSION_NAMESPACE { diff --git a/usd/src/usd_parser/USD.cc b/usd/src/usd_parser/USDWorld.cc similarity index 70% rename from usd/src/usd_parser/USD.cc rename to usd/src/usd_parser/USDWorld.cc index 1f1160e39..21f696c0f 100644 --- a/usd/src/usd_parser/USD.cc +++ b/usd/src/usd_parser/USDWorld.cc @@ -14,51 +14,56 @@ * limitations under the License. * */ -#include "USD.hh" +#include "USDWorld.hh" -#include "sdf/usd/usd_parser/USDData.hh" -#include "sdf/usd/usd_parser/USDStage.hh" -#include "USDPhysics.hh" +#include #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED #include #pragma pop_macro ("__DEPRECATED") -#include +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/usd/usd_parser/USDStage.hh" +#include "USDPhysics.hh" namespace sdf { inline namespace SDF_VERSION_NAMESPACE { namespace usd { - UsdErrors parseUSDWorld( - const std::string &_inputFilename, + UsdErrors parseUSDWorld(const std::string &_inputFileName, std::shared_ptr &_world) { UsdErrors errors; - USDData usdData(_inputFilename); + USDData usdData(_inputFileName); usdData.Init(); usdData.ParseMaterials(); - auto referencee = pxr::UsdStage::Open(_inputFilename); - if (!referencee) + auto reference = pxr::UsdStage::Open(_inputFileName); + if (!reference) { errors.emplace_back(UsdError( UsdErrorCode::INVALID_USD_FILE, - "Unable to open [" + _inputFilename + "]")); + "Unable to open [" + _inputFileName + "]")); return errors; } - auto range = pxr::UsdPrimRange::Stage(referencee); - - _world->worldName = referencee->GetDefaultPrim().GetName().GetText(); + _world->worldName = reference->GetDefaultPrim().GetName().GetText(); + auto range = pxr::UsdPrimRange::Stage(reference); for (auto const &prim : range) { if (prim.IsA()) { std::pair> data = usdData.FindStage(prim.GetPath().GetName()); + if (!data.second) + { + errors.push_back(UsdError(UsdErrorCode::INVALID_PRIM_PATH, + "Unable to retrieve the pxr::UsdPhysicsScene named [" + + prim.GetPath().GetName() + "]")); + return errors; + } ParseUSDPhysicsScene(prim, _world, data.second->MetersPerUnit()); continue; diff --git a/usd/src/usd_parser/USD.hh b/usd/src/usd_parser/USDWorld.hh similarity index 71% rename from usd/src/usd_parser/USD.hh rename to usd/src/usd_parser/USDWorld.hh index 38171d912..73b604c8d 100644 --- a/usd/src/usd_parser/USD.hh +++ b/usd/src/usd_parser/USDWorld.hh @@ -15,8 +15,8 @@ * */ -#ifndef USD_PARSER_USD_HH -#define USD_PARSER_USD_HH +#ifndef USD_PARSER_USDWORLD_HH +#define USD_PARSER_USDWORLD_HH #include @@ -27,19 +27,19 @@ namespace sdf { - // Inline bracke to help doxygen filtering. + // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // namespace usd { - /// \brief it parses the USD file - /// \param[in] _inputFilename Path where the USD is located - /// \param[out] _world World interface where all USD data is placed + /// \brief Parse the world information of a USD file + /// \param[in] _inputFileNameUsd Path where the USD is located + /// \param[out] _world World interface where all USD world data is placed /// \return UsdErrors, which is a vector of UsdError objects. Each UsdError /// includes an error code and message. An empty vector indicates no error - /// occurred when parsing to its SDF representation. + /// occurred when parsing the world information of _inputFileNameUsd UsdErrors parseUSDWorld( - const std::string &_inputFilename, + const std::string &_inputFileNameUsd, std::shared_ptr &_world); } } From 460d6938000d26368ca1d4dcdbfc9dc7c13e743d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 10 Mar 2022 10:14:36 +0100 Subject: [PATCH 08/33] feedback Signed-off-by: ahcorde --- usd/src/usd_parser/USDWorld.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/usd/src/usd_parser/USDWorld.cc b/usd/src/usd_parser/USDWorld.cc index 21f696c0f..19995fe60 100644 --- a/usd/src/usd_parser/USDWorld.cc +++ b/usd/src/usd_parser/USDWorld.cc @@ -53,15 +53,17 @@ namespace usd auto range = pxr::UsdPrimRange::Stage(reference); for (auto const &prim : range) { + std::string primName = prim.GetName(); + if (prim.IsA()) { std::pair> data = - usdData.FindStage(prim.GetPath().GetName()); + usdData.FindStage(primName); if (!data.second) { errors.push_back(UsdError(UsdErrorCode::INVALID_PRIM_PATH, "Unable to retrieve the pxr::UsdPhysicsScene named [" - + prim.GetPath().GetName() + "]")); + + primName + "]")); return errors; } From a01e4f9a9ba3ec7a2bdd02e00a235c1602df9e8b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 10 Mar 2022 13:39:18 +0100 Subject: [PATCH 09/33] Fix when physics schema is not available Signed-off-by: ahcorde --- test/usd/upAxisY.usda | 6 ---- usd/src/usd_parser/USDPhysics.cc | 34 +++++++++++++++---- usd/src/usd_parser/USDPhysics_TEST.cc | 16 ++++++++- .../usd_parser/usd_model/WorldInterface.hh | 4 +-- 4 files changed, 44 insertions(+), 16 deletions(-) diff --git a/test/usd/upAxisY.usda b/test/usd/upAxisY.usda index 90bece191..000ea5db7 100644 --- a/test/usd/upAxisY.usda +++ b/test/usd/upAxisY.usda @@ -6,12 +6,6 @@ def "shapes" { - def PhysicsScene "physics" - { - vector3f physics:gravityDirection = (0, 0, -1) - float physics:gravityMagnitude = 9.8 - } - def Xform "ground_plane" { float3 xformOp:rotateXYZ = (0, 0, 0) diff --git a/usd/src/usd_parser/USDPhysics.cc b/usd/src/usd_parser/USDPhysics.cc index 8bf8f627e..daf2a75d4 100644 --- a/usd/src/usd_parser/USDPhysics.cc +++ b/usd/src/usd_parser/USDPhysics.cc @@ -36,13 +36,33 @@ namespace usd double _metersPerUnit) { auto variant_physics_scene = pxr::UsdPhysicsScene(_prim); - pxr::GfVec3f gravity; - variant_physics_scene.GetGravityDirectionAttr().Get(&gravity); - _world->gravity[0] = gravity[0]; - _world->gravity[1] = gravity[1]; - _world->gravity[2] = gravity[2]; - variant_physics_scene.GetGravityMagnitudeAttr().Get(&_world->magnitude); - _world->magnitude *= _metersPerUnit; + + auto gravityAttr = variant_physics_scene.GetGravityDirectionAttr(); + if (gravityAttr) + { + pxr::GfVec3f gravity; + gravityAttr.Get(&gravity); + if (!ignition::math::equal(0.0f, gravity[0]) && + !ignition::math::equal(0.0f, gravity[1]) && + !ignition::math::equal(0.0f, gravity[2])) + { + _world->gravity[0] = gravity[0]; + _world->gravity[1] = gravity[1]; + _world->gravity[2] = gravity[2]; + } + } + + auto magnitudeAttr = variant_physics_scene.GetGravityMagnitudeAttr(); + if (magnitudeAttr) + { + float magnitude; + magnitudeAttr.Get(&magnitude); + if (!std::isnan(magnitude) && !std::isinf(magnitude) ) + { + std::cerr << "magnitudeAttr" << '\n'; + _world->magnitude = magnitude * _metersPerUnit; + } + } } } } diff --git a/usd/src/usd_parser/USDPhysics_TEST.cc b/usd/src/usd_parser/USDPhysics_TEST.cc index ef6085932..bbccf98c5 100644 --- a/usd/src/usd_parser/USDPhysics_TEST.cc +++ b/usd/src/usd_parser/USDPhysics_TEST.cc @@ -37,7 +37,7 @@ #include "usd_model/WorldInterface.hh" ///////////////////////////////////////////////// -TEST(USDLightsTest, DistanceLight) +TEST(USDPhysicsTest, AvailablePhysics) { std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); auto stage = pxr::UsdStage::Open(filename); @@ -55,3 +55,17 @@ TEST(USDLightsTest, DistanceLight) EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface->gravity); EXPECT_NEAR(9.8, worldInterface->magnitude, 0.0001); } + +///////////////////////////////////////////////// +TEST(USDPhysicsTest, UnavailablePhysics) +{ + std::string filename = sdf::testing::TestFile("usd", "upAxisY.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + std::shared_ptr worldInterface = + std::make_shared(); + + EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface->gravity); + EXPECT_NEAR(9.8, worldInterface->magnitude, 0.0001); +} diff --git a/usd/src/usd_parser/usd_model/WorldInterface.hh b/usd/src/usd_parser/usd_model/WorldInterface.hh index cb59b21e4..18889ce43 100644 --- a/usd/src/usd_parser/usd_model/WorldInterface.hh +++ b/usd/src/usd_parser/usd_model/WorldInterface.hh @@ -41,10 +41,10 @@ namespace sdf std::string worldName; /// \brief Magnitude of the gravity - float magnitude; + float magnitude {9.8f}; /// \brief Gravity (X, Y, Z) - ignition::math::Vector3d gravity; + ignition::math::Vector3d gravity {0.0, 0.0, -1.0}; friend std::ostream& operator<<( std::ostream& os, const WorldInterface& _world) From 318eff97b9c1459683b304cdeef87c68f1014336 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Thu, 10 Mar 2022 19:29:47 -0700 Subject: [PATCH 10/33] review feedback Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- usd/src/cmd/usd2sdf.cc | 10 +++- usd/src/usd_parser/USD2SDF.cc | 37 ++++++------- usd/src/usd_parser/USD2SDF.hh | 39 +++++++------- usd/src/usd_parser/USDPhysics.cc | 15 ++---- usd/src/usd_parser/USDPhysics.hh | 12 ++--- usd/src/usd_parser/USDPhysics_TEST.cc | 31 +++++++---- usd/src/usd_parser/USDWorld.cc | 4 +- usd/src/usd_parser/usd2sdf_TEST.cc | 54 +++++++++++-------- .../usd_parser/usd_model/WorldInterface.hh | 11 ++-- 9 files changed, 116 insertions(+), 97 deletions(-) diff --git a/usd/src/cmd/usd2sdf.cc b/usd/src/cmd/usd2sdf.cc index 41dc3288c..70edcae11 100644 --- a/usd/src/cmd/usd2sdf.cc +++ b/usd/src/cmd/usd2sdf.cc @@ -45,7 +45,15 @@ struct Options void runCommand(const Options &_opt) { - sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename); + const auto errors = + sdf::usd::parseUSDFile(_opt.inputFilename, _opt.outputFilename); + if (!errors.empty()) + { + std::cerr << "Errors occurred when generating [" << _opt.outputFilename + << "] from [" << _opt.inputFilename << "]:\n"; + for (const auto &e : errors) + std::cerr << "\t" << e << "\n"; + } } void addFlags(CLI::App &_app) diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc index 488fbd802..fbb9326cc 100644 --- a/usd/src/usd_parser/USD2SDF.cc +++ b/usd/src/usd_parser/USD2SDF.cc @@ -17,8 +17,9 @@ #include "USD2SDF.hh" +#include "sdf/Console.hh" +#include "sdf/Types.hh" #include "usd_model/WorldInterface.hh" - #include "USDWorld.hh" namespace sdf { @@ -35,30 +36,28 @@ USD2SDF::~USD2SDF() } ///////////////////////////////////////////////// -UsdErrors USD2SDF::Read(const std::string &_filename, - tinyxml2::XMLDocument* _sdfXmlOut) +UsdErrors USD2SDF::Read(const std::string &_fileName, + tinyxml2::XMLDocument *_sdfXmlOut) { UsdErrors errors; std::shared_ptr worldInterface = std::make_shared(); - auto errorsParseUSD = - parseUSDWorld(_filename, worldInterface); + const auto errorsParseUSD = parseUSDWorld(_fileName, worldInterface); if (!errorsParseUSD.empty()) { errors.emplace_back(UsdError( UsdErrorCode::SDF_TO_USD_PARSING_ERROR, - "Error parsing the usd file")); + "Error parsing usd file [" + _fileName + "]")); return errors; } tinyxml2::XMLElement *sdf = nullptr; - tinyxml2::XMLElement *world = nullptr; - sdf = _sdfXmlOut->NewElement("sdf"); sdf->SetAttribute("version", "1.7"); + tinyxml2::XMLElement *world = nullptr; world = _sdfXmlOut->NewElement("world"); std::string worldName = worldInterface->worldName; if (worldName.empty()) @@ -67,7 +66,7 @@ UsdErrors USD2SDF::Read(const std::string &_filename, } world->SetAttribute("name", (worldName + "_world").c_str()); - AddKeyValue(world, "gravity", Vector32Str( + this->AddKeyValue(world, "gravity", Vector32Str( worldInterface->gravity * worldInterface->magnitude)); sdf->LinkEndChild(world); @@ -75,8 +74,9 @@ UsdErrors USD2SDF::Read(const std::string &_filename, return errors; } -//////////////////////////////////////////////////////////////////////////////// -std::string USD2SDF::GetKeyValueAsString(tinyxml2::XMLElement* _elem) +///////////////////////////////////////////////// +std::string USD2SDF::GetKeyValueAsString( + const tinyxml2::XMLElement *_elem) const { std::string valueStr; if (_elem->Attribute("value")) @@ -98,7 +98,7 @@ std::string USD2SDF::GetKeyValueAsString(tinyxml2::XMLElement* _elem) return trim(valueStr); } -//////////////////////////////////////////////////////////////////////////////// +///////////////////////////////////////////////// void USD2SDF::AddKeyValue( tinyxml2::XMLElement *_elem, const std::string &_key, @@ -107,7 +107,7 @@ void USD2SDF::AddKeyValue( tinyxml2::XMLElement *childElem = _elem->FirstChildElement(_key.c_str()); if (childElem) { - std::string oldValue = GetKeyValueAsString(childElem); + std::string oldValue = this->GetKeyValueAsString(childElem); if (oldValue != _value) { sdferr << "multiple inconsistent <" << _key @@ -121,7 +121,8 @@ void USD2SDF::AddKeyValue( << "> exists with [" << _value << "] due to fixed joint reduction.\n"; } - _elem->DeleteChild(childElem); // remove old _elem + // remove old _elem + _elem->DeleteChild(childElem); } auto *doc = _elem->GetDocument(); @@ -132,14 +133,10 @@ void USD2SDF::AddKeyValue( } ///////////////////////////////////////////////// -std::string USD2SDF::Vector32Str(const ignition::math::Vector3d _vector) +std::string USD2SDF::Vector32Str(const ignition::math::Vector3d &_vector) const { std::stringstream ss; - ss << _vector.X(); - ss << " "; - ss << _vector.Y(); - ss << " "; - ss << _vector.Z(); + ss << _vector; return ss.str(); } } diff --git a/usd/src/usd_parser/USD2SDF.hh b/usd/src/usd_parser/USD2SDF.hh index 52f64c322..aab66cde0 100644 --- a/usd/src/usd_parser/USD2SDF.hh +++ b/usd/src/usd_parser/USD2SDF.hh @@ -15,27 +15,25 @@ * */ -#ifndef SDF_USD_USD_PARSER_USD2SDF_HH -#define SDF_USD_USD_PARSER_USD2SDF_HH +#ifndef USD_PARSER_USD2SDF_HH_ +#define USD_PARSER_USD2SDF_HH_ #include #include - #include -#include - +#include "sdf/sdf_config.h" #include "sdf/usd/UsdError.hh" namespace sdf { - // Inline bracket to help doxygen filtering. - inline namespace SDF_VERSION_NAMESPACE { +// Inline bracket to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { + namespace usd + { /// \brief USD to SDF converter - /// This class helps to generate the SDF file - namespace usd - { + /// This class helps generate the SDF file class USD2SDF { /// \brief constructor @@ -45,16 +43,20 @@ namespace sdf public: ~USD2SDF(); /// \brief convert USD file to sdf xml document - /// \param[in] _filename string containing filename of the urdf model. - /// \param[inout] _sdfXmlDoc Document to populate with the sdf model. + /// \param[in] _fileMame string containing USD filename. + /// \param[out] _sdfXmlDoc Document to populate with the sdf model. + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means no errors occurred when populating _sdfXmlOut with the contents + /// of _fileName public: UsdErrors Read( - const std::string &_filename, - tinyxml2::XMLDocument* _sdfXmlOut); + const std::string &_fileName, + tinyxml2::XMLDocument *_sdfXmlOut); /// \brief get value from pair and return it as string /// \param[in] _elem pointer to xml element /// return a string with the key - private: std::string GetKeyValueAsString(tinyxml2::XMLElement* _elem); + private: std::string GetKeyValueAsString( + const tinyxml2::XMLElement *_elem) const; /// \brief append key value pair to the end of the xml element /// \param[in] _elem pointer to xml element @@ -67,11 +69,12 @@ namespace sdf /// \brief convert Vector3 to string /// \param[in] _vector a ignition::math::Vector3d - /// \return a string - private: std::string Vector32Str(const ignition::math::Vector3d _vector); + /// \return string representation of Vector3 + private: std::string Vector32Str( + const ignition::math::Vector3d &_vector) const; }; } - } +} } #endif diff --git a/usd/src/usd_parser/USDPhysics.cc b/usd/src/usd_parser/USDPhysics.cc index daf2a75d4..107b97c07 100644 --- a/usd/src/usd_parser/USDPhysics.cc +++ b/usd/src/usd_parser/USDPhysics.cc @@ -23,22 +23,17 @@ #include #pragma pop_macro ("__DEPRECATED") -#include "sdf/Console.hh" - namespace sdf { inline namespace SDF_VERSION_NAMESPACE { namespace usd { void ParseUSDPhysicsScene( - const pxr::UsdPrim &_prim, + const pxr::UsdPhysicsScene &_scene, std::shared_ptr &_world, double _metersPerUnit) { - auto variant_physics_scene = pxr::UsdPhysicsScene(_prim); - - auto gravityAttr = variant_physics_scene.GetGravityDirectionAttr(); - if (gravityAttr) + if (const auto gravityAttr = _scene.GetGravityDirectionAttr()) { pxr::GfVec3f gravity; gravityAttr.Get(&gravity); @@ -52,14 +47,12 @@ namespace usd } } - auto magnitudeAttr = variant_physics_scene.GetGravityMagnitudeAttr(); - if (magnitudeAttr) + if (const auto magnitudeAttr = _scene.GetGravityMagnitudeAttr()) { float magnitude; magnitudeAttr.Get(&magnitude); - if (!std::isnan(magnitude) && !std::isinf(magnitude) ) + if (!std::isnan(magnitude) && !std::isinf(magnitude)) { - std::cerr << "magnitudeAttr" << '\n'; _world->magnitude = magnitude * _metersPerUnit; } } diff --git a/usd/src/usd_parser/USDPhysics.hh b/usd/src/usd_parser/USDPhysics.hh index 1dc392a98..ac2ccc8ef 100644 --- a/usd/src/usd_parser/USDPhysics.hh +++ b/usd/src/usd_parser/USDPhysics.hh @@ -22,7 +22,7 @@ #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED -#include +#include #pragma pop_macro ("__DEPRECATED") #include "sdf/config.hh" @@ -30,17 +30,17 @@ namespace sdf { - // Inline bracke to help doxygen filtering. + // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // namespace usd { - /// \brief It parses the physics attributes of the USD file - /// \param[in] _prim Prim to extract the physics attributes + /// \brief Parse the physics attributes of a USD file + /// \param[in] _scene USD physics scene to extract attributes from /// \param[out] _world World interface where the data is placed - /// \param[in] _metersPerUnit meter per unit in the USD + /// \param[in] _metersPerUnit meters per unit in the USD void IGNITION_SDFORMAT_USD_VISIBLE ParseUSDPhysicsScene( - const pxr::UsdPrim &_prim, + const pxr::UsdPhysicsScene &_scene, std::shared_ptr &_world, double _metersPerUnit); } diff --git a/usd/src/usd_parser/USDPhysics_TEST.cc b/usd/src/usd_parser/USDPhysics_TEST.cc index bbccf98c5..3f42cbab3 100644 --- a/usd/src/usd_parser/USDPhysics_TEST.cc +++ b/usd/src/usd_parser/USDPhysics_TEST.cc @@ -20,13 +20,12 @@ #include #include -// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings // are reported using #pragma message so normal diagnostic flags cannot remove // them. This workaround requires this block to be used whenever usd is // included. #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED -#include #include #pragma pop_macro ("__DEPRECATED") @@ -39,33 +38,43 @@ ///////////////////////////////////////////////// TEST(USDPhysicsTest, AvailablePhysics) { - std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); - auto stage = pxr::UsdStage::Open(filename); + const std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + const auto stage = pxr::UsdStage::Open(filename); ASSERT_TRUE(stage); - pxr::UsdPrim prim = stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics")); + const auto physicsScene = + pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics"))); + EXPECT_TRUE(physicsScene); std::shared_ptr worldInterface = std::make_shared(); - double metersPerUnit = 1.0; + const double metersPerUnit = 1.0; sdf::usd::ParseUSDPhysicsScene( - prim, worldInterface, metersPerUnit); + physicsScene, worldInterface, metersPerUnit); EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface->gravity); - EXPECT_NEAR(9.8, worldInterface->magnitude, 0.0001); + EXPECT_FLOAT_EQ(9.8f, worldInterface->magnitude); } ///////////////////////////////////////////////// TEST(USDPhysicsTest, UnavailablePhysics) { - std::string filename = sdf::testing::TestFile("usd", "upAxisY.usda"); - auto stage = pxr::UsdStage::Open(filename); + const std::string filename = sdf::testing::TestFile("usd", "upAxisY.usda"); + const auto stage = pxr::UsdStage::Open(filename); ASSERT_TRUE(stage); + const auto physicsScene = + pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics"))); + EXPECT_FALSE(physicsScene); + std::shared_ptr worldInterface = std::make_shared(); + const double metersPerUnit = 1.0; + + sdf::usd::ParseUSDPhysicsScene( + physicsScene, worldInterface, metersPerUnit); EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface->gravity); - EXPECT_NEAR(9.8, worldInterface->magnitude, 0.0001); + EXPECT_FLOAT_EQ(9.8f, worldInterface->magnitude); } diff --git a/usd/src/usd_parser/USDWorld.cc b/usd/src/usd_parser/USDWorld.cc index 19995fe60..c0fc107ec 100644 --- a/usd/src/usd_parser/USDWorld.cc +++ b/usd/src/usd_parser/USDWorld.cc @@ -20,6 +20,7 @@ #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED +#include #include #pragma pop_macro ("__DEPRECATED") @@ -67,7 +68,8 @@ namespace usd return errors; } - ParseUSDPhysicsScene(prim, _world, data.second->MetersPerUnit()); + ParseUSDPhysicsScene(pxr::UsdPhysicsScene(prim), _world, + data.second->MetersPerUnit()); continue; } } diff --git a/usd/src/usd_parser/usd2sdf_TEST.cc b/usd/src/usd_parser/usd2sdf_TEST.cc index d334d94aa..fc73df3a1 100644 --- a/usd/src/usd_parser/usd2sdf_TEST.cc +++ b/usd/src/usd_parser/usd2sdf_TEST.cc @@ -23,6 +23,8 @@ #include +#include "sdf/Root.hh" +#include "sdf/World.hh" #include "test_config.h" #include "test_utils.hh" @@ -62,36 +64,42 @@ std::string custom_exec_str(std::string _cmd) ///////////////////////////////////////////////// TEST(version_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - // Check a good SDF file - { - std::string output = - custom_exec_str(usd2sdfCommand() + " --version"); - - EXPECT_EQ(output, std::string(SDF_VERSION_FULL) + "\n"); - - // TODO(ahcorde): Check the contents of outputUsdFilePath when the parser - // is implemented - } + std::string output = + custom_exec_str(usd2sdfCommand() + " --version"); + EXPECT_EQ(output, std::string(SDF_VERSION_FULL) + "\n"); } ///////////////////////////////////////////////// TEST(check_cmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) { - auto tmpDir = ignition::common::tempDirectoryPath(); - auto tmp = ignition::common::createTempDirectory("usd", tmpDir); + const auto tmp = ignition::common::createTempDirectory("usd", + ignition::common::tempDirectoryPath()); // Check a good SDF file { - std::string path = sdf::testing::TestFile("usd", "upAxisZ.usda"); - const auto outputUsdFilePath = + const std::string path = sdf::testing::TestFile("usd", "upAxisZ.usda"); + const auto outputSdfFilePath = ignition::common::joinPaths(tmp, "upAxisZ.sdf"); - EXPECT_FALSE(ignition::common::isFile(outputUsdFilePath)); - std::string output = - custom_exec_str(usd2sdfCommand() + " " + path + " " + outputUsdFilePath); - - // make sure that a shapes.usd file was generated - EXPECT_TRUE(ignition::common::isFile(outputUsdFilePath)) << output; - - // TODO(anyone): Check the contents of outputUsdFilePath when the parser - // is implemented + EXPECT_FALSE(ignition::common::isFile(outputSdfFilePath)); + const std::string output = + custom_exec_str(usd2sdfCommand() + " " + path + " " + outputSdfFilePath); + + // make sure that a sdf file was generated + ASSERT_TRUE(ignition::common::isFile(outputSdfFilePath)) << output; + + // check the contents of the generated SDF file + sdf::Root root; + const auto errors = root.Load(outputSdfFilePath); + EXPECT_TRUE(errors.empty()); + + // check the value of the gravity element + ASSERT_EQ(1u, root.WorldCount()); + const auto world = root.WorldByIndex(0u); + ASSERT_NE(nullptr, world); + EXPECT_DOUBLE_EQ(0.0, world->Gravity()[0]); + EXPECT_DOUBLE_EQ(0.0, world->Gravity()[1]); + EXPECT_DOUBLE_EQ(-0.098, world->Gravity()[2]); + + // TODO(anyone) Check the remaining contents of outputUsdFilePath + // when the parser is implemented } } diff --git a/usd/src/usd_parser/usd_model/WorldInterface.hh b/usd/src/usd_parser/usd_model/WorldInterface.hh index 18889ce43..1584b7df7 100644 --- a/usd/src/usd_parser/usd_model/WorldInterface.hh +++ b/usd/src/usd_parser/usd_model/WorldInterface.hh @@ -24,17 +24,16 @@ #include -#include -#include +#include "sdf/sdf_config.h" namespace sdf { - // Inline bracke to help doxygen filtering. + // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // namespace usd { - /// \brief This class store data about the world + /// \brief This class stores data about the world class WorldInterface { public: /// \brief World name @@ -49,8 +48,8 @@ namespace sdf friend std::ostream& operator<<( std::ostream& os, const WorldInterface& _world) { - os << "World name: " << _world.worldName; - os << "Gravity: " << _world.gravity * _world.magnitude << "\n"; + os << "World name: " << _world.worldName + << ", Gravity: " << _world.gravity * _world.magnitude; return os; } }; From 602a176e8aa85dfc37c765e62a95042c09874388 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Thu, 10 Mar 2022 22:24:08 -0700 Subject: [PATCH 11/33] fix test Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- usd/src/usd_parser/USDStage_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/usd/src/usd_parser/USDStage_TEST.cc b/usd/src/usd_parser/USDStage_TEST.cc index dcb4f814a..246e8fb34 100644 --- a/usd/src/usd_parser/USDStage_TEST.cc +++ b/usd/src/usd_parser/USDStage_TEST.cc @@ -51,7 +51,7 @@ TEST(USDStage, Constructor) EXPECT_EQ("Y", stage.UpAxis()); EXPECT_DOUBLE_EQ(1.0, stage.MetersPerUnit()); - EXPECT_EQ(10u, stage.USDPaths().size()); + EXPECT_EQ(9u, stage.USDPaths().size()); } // Wrong upaxis From 2ba1b26b481b38f1561e7d948efdb63db2bfc112 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 11 Mar 2022 23:50:32 +0100 Subject: [PATCH 12/33] Added some feedback Signed-off-by: ahcorde --- usd/src/cmd/usd2sdf.cc | 1 + usd/src/usd_parser/Parser.cc | 5 ++--- usd/src/usd_parser/USD2SDF.cc | 14 ++------------ usd/src/usd_parser/USD2SDF.hh | 5 +---- 4 files changed, 6 insertions(+), 19 deletions(-) diff --git a/usd/src/cmd/usd2sdf.cc b/usd/src/cmd/usd2sdf.cc index 70edcae11..43dd7b794 100644 --- a/usd/src/cmd/usd2sdf.cc +++ b/usd/src/cmd/usd2sdf.cc @@ -15,6 +15,7 @@ * */ +#include #include #include diff --git a/usd/src/usd_parser/Parser.cc b/usd/src/usd_parser/Parser.cc index 787a10ce8..ba54d273a 100644 --- a/usd/src/usd_parser/Parser.cc +++ b/usd/src/usd_parser/Parser.cc @@ -30,10 +30,9 @@ namespace usd UsdErrors errors; USD2SDF usd2sdf; auto doc = tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE); - auto readErrors = usd2sdf.Read(_inputFilenameUsd, &doc); - if (!readErrors.empty()) + errors = usd2sdf.Read(_inputFilenameUsd, &doc); + if (!errors.empty()) { - errors.insert(errors.end(), readErrors.begin(), readErrors.end()); return errors; } doc.SaveFile(_outputFilenameSdf.c_str()); diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc index fbb9326cc..86ded4f5c 100644 --- a/usd/src/usd_parser/USD2SDF.cc +++ b/usd/src/usd_parser/USD2SDF.cc @@ -25,17 +25,7 @@ namespace sdf { inline namespace SDF_VERSION_NAMESPACE { namespace usd { -///////////////////////////////////////////////// -USD2SDF::USD2SDF() -{ -} - -///////////////////////////////////////////////// -USD2SDF::~USD2SDF() -{ -} - -///////////////////////////////////////////////// +//////////////////////////////////////////////// UsdErrors USD2SDF::Read(const std::string &_fileName, tinyxml2::XMLDocument *_sdfXmlOut) { @@ -55,7 +45,7 @@ UsdErrors USD2SDF::Read(const std::string &_fileName, tinyxml2::XMLElement *sdf = nullptr; sdf = _sdfXmlOut->NewElement("sdf"); - sdf->SetAttribute("version", "1.7"); + sdf->SetAttribute("version", SDF_PROTOCOL_VERSION); tinyxml2::XMLElement *world = nullptr; world = _sdfXmlOut->NewElement("world"); diff --git a/usd/src/usd_parser/USD2SDF.hh b/usd/src/usd_parser/USD2SDF.hh index aab66cde0..af8d96eca 100644 --- a/usd/src/usd_parser/USD2SDF.hh +++ b/usd/src/usd_parser/USD2SDF.hh @@ -37,10 +37,7 @@ inline namespace SDF_VERSION_NAMESPACE { class USD2SDF { /// \brief constructor - public: USD2SDF(); - - /// \brief destructor - public: ~USD2SDF(); + public: USD2SDF() = default; /// \brief convert USD file to sdf xml document /// \param[in] _fileMame string containing USD filename. From d8212aeaf7e96c256b4172e9a84a58dacea6d27a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 14 Mar 2022 12:47:16 +0100 Subject: [PATCH 13/33] Added feedback Signed-off-by: ahcorde --- usd/src/usd_parser/Parser.cc | 19 +++++- usd/src/usd_parser/USD2SDF.cc | 97 +++++---------------------- usd/src/usd_parser/USD2SDF.hh | 28 +------- usd/src/usd_parser/USDPhysics.cc | 34 +++++----- usd/src/usd_parser/USDPhysics.hh | 2 +- usd/src/usd_parser/USDPhysics_TEST.cc | 14 ++-- usd/src/usd_parser/USDWorld.cc | 4 +- usd/src/usd_parser/USDWorld.hh | 2 +- 8 files changed, 62 insertions(+), 138 deletions(-) diff --git a/usd/src/usd_parser/Parser.cc b/usd/src/usd_parser/Parser.cc index ba54d273a..c9a9c8190 100644 --- a/usd/src/usd_parser/Parser.cc +++ b/usd/src/usd_parser/Parser.cc @@ -18,6 +18,8 @@ #include "sdf/usd/usd_parser/Parser.hh" #include "USD2SDF.hh" +#include "sdf/Root.hh" + namespace sdf { inline namespace SDF_VERSION_NAMESPACE { @@ -29,13 +31,24 @@ namespace usd { UsdErrors errors; USD2SDF usd2sdf; - auto doc = tinyxml2::XMLDocument(true, tinyxml2::COLLAPSE_WHITESPACE); - errors = usd2sdf.Read(_inputFilenameUsd, &doc); + sdf::Root root; + errors = usd2sdf.Read(_inputFilenameUsd, root); if (!errors.empty()) { return errors; } - doc.SaveFile(_outputFilenameSdf.c_str()); + + std::ofstream out(_outputFilenameSdf.c_str(), std::ios::out); + std::string string = root.ToElement()->ToString(""); + if (!out) + { + errors.emplace_back(UsdError( + UsdErrorCode::SDF_TO_USD_PARSING_ERROR, + "Unable to open file [" + _outputFilenameSdf + "] for writing")); + return errors; + } + out << string; + out.close(); return errors; } } diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc index 86ded4f5c..9b70cf1c8 100644 --- a/usd/src/usd_parser/USD2SDF.cc +++ b/usd/src/usd_parser/USD2SDF.cc @@ -22,17 +22,19 @@ #include "usd_model/WorldInterface.hh" #include "USDWorld.hh" +#include "sdf/Root.hh" +#include "sdf/World.hh" + namespace sdf { inline namespace SDF_VERSION_NAMESPACE { namespace usd { //////////////////////////////////////////////// UsdErrors USD2SDF::Read(const std::string &_fileName, - tinyxml2::XMLDocument *_sdfXmlOut) + sdf::Root &_root) { UsdErrors errors; - std::shared_ptr worldInterface = - std::make_shared(); + WorldInterface worldInterface; const auto errorsParseUSD = parseUSDWorld(_fileName, worldInterface); if (!errorsParseUSD.empty()) @@ -43,91 +45,28 @@ UsdErrors USD2SDF::Read(const std::string &_fileName, return errors; } - tinyxml2::XMLElement *sdf = nullptr; - sdf = _sdfXmlOut->NewElement("sdf"); - sdf->SetAttribute("version", SDF_PROTOCOL_VERSION); + sdf::World world; - tinyxml2::XMLElement *world = nullptr; - world = _sdfXmlOut->NewElement("world"); - std::string worldName = worldInterface->worldName; + std::string worldName = worldInterface.worldName; if (worldName.empty()) { - worldName = "world_name"; - } - world->SetAttribute("name", (worldName + "_world").c_str()); - - this->AddKeyValue(world, "gravity", Vector32Str( - worldInterface->gravity * worldInterface->magnitude)); - - sdf->LinkEndChild(world); - _sdfXmlOut->LinkEndChild(sdf); - return errors; -} - -///////////////////////////////////////////////// -std::string USD2SDF::GetKeyValueAsString( - const tinyxml2::XMLElement *_elem) const -{ - std::string valueStr; - if (_elem->Attribute("value")) - { - valueStr = _elem->Attribute("value"); + world.SetName("world_name"); } - else if (_elem->FirstChild()) + else { - // Check that this node is a XMLText - if (_elem->FirstChild()->ToText()) - { - valueStr = _elem->FirstChild()->Value(); - } - else - { - sdfwarn << "Attribute value string not set\n"; - } + world.SetName(worldName + "_world"); } - return trim(valueStr); -} - -///////////////////////////////////////////////// -void USD2SDF::AddKeyValue( - tinyxml2::XMLElement *_elem, - const std::string &_key, - const std::string &_value) -{ - tinyxml2::XMLElement *childElem = _elem->FirstChildElement(_key.c_str()); - if (childElem) + auto addWorldErrors = _root.AddWorld(world); + if (!addWorldErrors.empty()) { - std::string oldValue = this->GetKeyValueAsString(childElem); - if (oldValue != _value) - { - sdferr << "multiple inconsistent <" << _key - << "> exists due to fixed joint reduction" - << " overwriting previous value [" << oldValue - << "] with [" << _value << "].\n"; - } - else - { - sdferr << "multiple consistent <" << _key - << "> exists with [" << _value - << "] due to fixed joint reduction.\n"; - } - // remove old _elem - _elem->DeleteChild(childElem); + errors.emplace_back(UsdError( + UsdErrorCode::SDF_ERROR, + "Error adding the world [" + worldName + "]")); + return errors; } + world.SetGravity(worldInterface.gravity * worldInterface.magnitude); - auto *doc = _elem->GetDocument(); - tinyxml2::XMLElement *ekey = doc->NewElement(_key.c_str()); - tinyxml2::XMLText *textEkey = doc->NewText(_value.c_str()); - ekey->LinkEndChild(textEkey); - _elem->LinkEndChild(ekey); -} - -///////////////////////////////////////////////// -std::string USD2SDF::Vector32Str(const ignition::math::Vector3d &_vector) const -{ - std::stringstream ss; - ss << _vector; - return ss.str(); + return errors; } } } diff --git a/usd/src/usd_parser/USD2SDF.hh b/usd/src/usd_parser/USD2SDF.hh index af8d96eca..e4294e404 100644 --- a/usd/src/usd_parser/USD2SDF.hh +++ b/usd/src/usd_parser/USD2SDF.hh @@ -20,12 +20,11 @@ #include -#include -#include - #include "sdf/sdf_config.h" #include "sdf/usd/UsdError.hh" +#include "sdf/Root.hh" + namespace sdf { // Inline bracket to help doxygen filtering. @@ -47,28 +46,7 @@ inline namespace SDF_VERSION_NAMESPACE { /// of _fileName public: UsdErrors Read( const std::string &_fileName, - tinyxml2::XMLDocument *_sdfXmlOut); - - /// \brief get value from pair and return it as string - /// \param[in] _elem pointer to xml element - /// return a string with the key - private: std::string GetKeyValueAsString( - const tinyxml2::XMLElement *_elem) const; - - /// \brief append key value pair to the end of the xml element - /// \param[in] _elem pointer to xml element - /// \param[in] _key string containing key to add to xml element - /// \param[in] _value string containing value for the key added - private: void AddKeyValue( - tinyxml2::XMLElement *_elem, - const std::string &_key, - const std::string &_value); - - /// \brief convert Vector3 to string - /// \param[in] _vector a ignition::math::Vector3d - /// \return string representation of Vector3 - private: std::string Vector32Str( - const ignition::math::Vector3d &_vector) const; + sdf::Root &_root); }; } } diff --git a/usd/src/usd_parser/USDPhysics.cc b/usd/src/usd_parser/USDPhysics.cc index 107b97c07..d63ce7d7a 100644 --- a/usd/src/usd_parser/USDPhysics.cc +++ b/usd/src/usd_parser/USDPhysics.cc @@ -30,31 +30,27 @@ namespace usd { void ParseUSDPhysicsScene( const pxr::UsdPhysicsScene &_scene, - std::shared_ptr &_world, + WorldInterface &_world, double _metersPerUnit) { - if (const auto gravityAttr = _scene.GetGravityDirectionAttr()) + const auto gravityAttr = _scene.GetGravityDirectionAttr(); + pxr::GfVec3f gravity; + gravityAttr.Get(&gravity); + if (!ignition::math::equal(0.0f, gravity[0]) && + !ignition::math::equal(0.0f, gravity[1]) && + !ignition::math::equal(0.0f, gravity[2])) { - pxr::GfVec3f gravity; - gravityAttr.Get(&gravity); - if (!ignition::math::equal(0.0f, gravity[0]) && - !ignition::math::equal(0.0f, gravity[1]) && - !ignition::math::equal(0.0f, gravity[2])) - { - _world->gravity[0] = gravity[0]; - _world->gravity[1] = gravity[1]; - _world->gravity[2] = gravity[2]; - } + _world.gravity[0] = gravity[0]; + _world.gravity[1] = gravity[1]; + _world.gravity[2] = gravity[2]; } - if (const auto magnitudeAttr = _scene.GetGravityMagnitudeAttr()) + const auto magnitudeAttr = _scene.GetGravityMagnitudeAttr(); + float magnitude; + magnitudeAttr.Get(&magnitude); + if (!std::isnan(magnitude) && !std::isinf(magnitude)) { - float magnitude; - magnitudeAttr.Get(&magnitude); - if (!std::isnan(magnitude) && !std::isinf(magnitude)) - { - _world->magnitude = magnitude * _metersPerUnit; - } + _world.magnitude = magnitude * _metersPerUnit; } } } diff --git a/usd/src/usd_parser/USDPhysics.hh b/usd/src/usd_parser/USDPhysics.hh index ac2ccc8ef..ee6627395 100644 --- a/usd/src/usd_parser/USDPhysics.hh +++ b/usd/src/usd_parser/USDPhysics.hh @@ -41,7 +41,7 @@ namespace sdf /// \param[in] _metersPerUnit meters per unit in the USD void IGNITION_SDFORMAT_USD_VISIBLE ParseUSDPhysicsScene( const pxr::UsdPhysicsScene &_scene, - std::shared_ptr &_world, + WorldInterface &_world, double _metersPerUnit); } } diff --git a/usd/src/usd_parser/USDPhysics_TEST.cc b/usd/src/usd_parser/USDPhysics_TEST.cc index 3f42cbab3..13ae67e91 100644 --- a/usd/src/usd_parser/USDPhysics_TEST.cc +++ b/usd/src/usd_parser/USDPhysics_TEST.cc @@ -46,15 +46,14 @@ TEST(USDPhysicsTest, AvailablePhysics) pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics"))); EXPECT_TRUE(physicsScene); - std::shared_ptr worldInterface = - std::make_shared(); + sdf::usd::WorldInterface worldInterface; const double metersPerUnit = 1.0; sdf::usd::ParseUSDPhysicsScene( physicsScene, worldInterface, metersPerUnit); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface->gravity); - EXPECT_FLOAT_EQ(9.8f, worldInterface->magnitude); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface.gravity); + EXPECT_FLOAT_EQ(9.8f, worldInterface.magnitude); } ///////////////////////////////////////////////// @@ -68,13 +67,12 @@ TEST(USDPhysicsTest, UnavailablePhysics) pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics"))); EXPECT_FALSE(physicsScene); - std::shared_ptr worldInterface = - std::make_shared(); + sdf::usd::WorldInterface worldInterface; const double metersPerUnit = 1.0; sdf::usd::ParseUSDPhysicsScene( physicsScene, worldInterface, metersPerUnit); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface->gravity); - EXPECT_FLOAT_EQ(9.8f, worldInterface->magnitude); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface.gravity); + EXPECT_FLOAT_EQ(9.8f, worldInterface.magnitude); } diff --git a/usd/src/usd_parser/USDWorld.cc b/usd/src/usd_parser/USDWorld.cc index c0fc107ec..9957aeb9c 100644 --- a/usd/src/usd_parser/USDWorld.cc +++ b/usd/src/usd_parser/USDWorld.cc @@ -34,7 +34,7 @@ inline namespace SDF_VERSION_NAMESPACE { namespace usd { UsdErrors parseUSDWorld(const std::string &_inputFileName, - std::shared_ptr &_world) + WorldInterface &_world) { UsdErrors errors; USDData usdData(_inputFileName); @@ -49,7 +49,7 @@ namespace usd "Unable to open [" + _inputFileName + "]")); return errors; } - _world->worldName = reference->GetDefaultPrim().GetName().GetText(); + _world.worldName = reference->GetDefaultPrim().GetName().GetText(); auto range = pxr::UsdPrimRange::Stage(reference); for (auto const &prim : range) diff --git a/usd/src/usd_parser/USDWorld.hh b/usd/src/usd_parser/USDWorld.hh index 73b604c8d..fef1639a6 100644 --- a/usd/src/usd_parser/USDWorld.hh +++ b/usd/src/usd_parser/USDWorld.hh @@ -40,7 +40,7 @@ namespace sdf /// occurred when parsing the world information of _inputFileNameUsd UsdErrors parseUSDWorld( const std::string &_inputFileNameUsd, - std::shared_ptr &_world); + WorldInterface &_world); } } } From 50eabdb966556d559c44092de6f553dff209d524 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 14 Mar 2022 12:50:49 +0100 Subject: [PATCH 14/33] Added feedback Signed-off-by: ahcorde --- usd/src/usd_parser/USDPhysics.cc | 30 +++++++++------ .../usd_parser/usd_model/WorldInterface.hh | 37 +++++++++---------- 2 files changed, 36 insertions(+), 31 deletions(-) diff --git a/usd/src/usd_parser/USDPhysics.cc b/usd/src/usd_parser/USDPhysics.cc index d63ce7d7a..6e5a92098 100644 --- a/usd/src/usd_parser/USDPhysics.cc +++ b/usd/src/usd_parser/USDPhysics.cc @@ -34,23 +34,29 @@ namespace usd double _metersPerUnit) { const auto gravityAttr = _scene.GetGravityDirectionAttr(); - pxr::GfVec3f gravity; - gravityAttr.Get(&gravity); - if (!ignition::math::equal(0.0f, gravity[0]) && - !ignition::math::equal(0.0f, gravity[1]) && - !ignition::math::equal(0.0f, gravity[2])) + if (gravityAttr) { - _world.gravity[0] = gravity[0]; - _world.gravity[1] = gravity[1]; - _world.gravity[2] = gravity[2]; + pxr::GfVec3f gravity; + gravityAttr.Get(&gravity); + if (!ignition::math::equal(0.0f, gravity[0]) && + !ignition::math::equal(0.0f, gravity[1]) && + !ignition::math::equal(0.0f, gravity[2])) + { + _world.gravity[0] = gravity[0]; + _world.gravity[1] = gravity[1]; + _world.gravity[2] = gravity[2]; + } } const auto magnitudeAttr = _scene.GetGravityMagnitudeAttr(); - float magnitude; - magnitudeAttr.Get(&magnitude); - if (!std::isnan(magnitude) && !std::isinf(magnitude)) + if (magnitudeAttr) { - _world.magnitude = magnitude * _metersPerUnit; + float magnitude; + magnitudeAttr.Get(&magnitude); + if (!std::isnan(magnitude) && !std::isinf(magnitude)) + { + _world.magnitude = magnitude * _metersPerUnit; + } } } } diff --git a/usd/src/usd_parser/usd_model/WorldInterface.hh b/usd/src/usd_parser/usd_model/WorldInterface.hh index 1584b7df7..6da6f9c31 100644 --- a/usd/src/usd_parser/usd_model/WorldInterface.hh +++ b/usd/src/usd_parser/usd_model/WorldInterface.hh @@ -33,25 +33,24 @@ namespace sdf // namespace usd { - /// \brief This class stores data about the world - class WorldInterface { - public: - /// \brief World name - std::string worldName; - - /// \brief Magnitude of the gravity - float magnitude {9.8f}; - - /// \brief Gravity (X, Y, Z) - ignition::math::Vector3d gravity {0.0, 0.0, -1.0}; - - friend std::ostream& operator<<( - std::ostream& os, const WorldInterface& _world) - { - os << "World name: " << _world.worldName - << ", Gravity: " << _world.gravity * _world.magnitude; - return os; - } + /// \brief This struct stores data about the world + struct WorldInterface { + /// \brief World name + std::string worldName; + + /// \brief Magnitude of the gravity + float magnitude {9.8f}; + + /// \brief Gravity (X, Y, Z) + ignition::math::Vector3d gravity {0.0, 0.0, -1.0}; + + friend std::ostream& operator<<( + std::ostream& os, const WorldInterface& _world) + { + os << "World name: " << _world.worldName + << ", Gravity: " << _world.gravity * _world.magnitude; + return os; + } }; } } From 539d7242687e843e76555ef3c18b510577616154 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 14 Mar 2022 13:26:34 +0100 Subject: [PATCH 15/33] Added feedback Signed-off-by: ahcorde --- test/usd/upAxisZ.usda | 4 +-- .../sdf/usd/usd_parser/USDTransforms.hh | 29 +++++++++---------- usd/src/usd_parser/USDTransforms.cc | 17 +++++++---- usd/src/usd_parser/USDTransforms_TEST.cc | 15 ++++++++-- 4 files changed, 40 insertions(+), 25 deletions(-) diff --git a/test/usd/upAxisZ.usda b/test/usd/upAxisZ.usda index 6933533c8..ea76aa750 100644 --- a/test/usd/upAxisZ.usda +++ b/test/usd/upAxisZ.usda @@ -172,9 +172,9 @@ def "shapes" def Xform "capsule_visual" { - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateZYX = (0, 0, 90) double3 xformOp:translate = (0, 0, 0) - uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] def Capsule "geometry" ( prepend apiSchemas = ["PhysicsCollisionAPI"] diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 22764d9d8..26c9f1633 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -53,27 +53,26 @@ namespace sdf /// in the schema /// Rotation is splitted in a vector because this might be defined /// as a rotation of 3 angles (ZYX, XYZ, etc). - class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms + struct IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms { - public: - /// \brief Scale of the schema - ignition::math::Vector3d scale{1, 1, 1}; + /// \brief Scale of the schema + ignition::math::Vector3d scale{1, 1, 1}; - /// \brief Rotation of the schema - std::vector q; + /// \brief Rotation of the schema + std::vector q; - /// \brief Translatio of the schema - ignition::math::Vector3d translate{0, 0, 0}; + /// \brief Translatio of the schema + ignition::math::Vector3d translate{0, 0, 0}; - /// \brief True if there is a rotation ZYX definedor false otherwise - bool isRotationZYX = false; + /// \brief True if there is a rotation ZYX defined or false otherwise + bool isRotationZYX = false; - /// \brief True if there is a rotation (as a quaterion) defined - /// or false otherwise - bool isRotation = false; + /// \brief True if there is a rotation XYZ defined or false otherwise + bool isRotationXYZ = false; - /// \brief True if there is a translation defined or false otherwise - bool isTranslate = false; + /// \brief True if there is a rotation (as a quaterion) defined + /// or false otherwise + bool isRotation = false; }; /// \brief This function will parse all the parents transforms of a prim diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index ed6e6f0ea..d26ca9a24 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -70,7 +70,7 @@ namespace sdf child.Pos().Z() * t.scale[2]); } - if (!t.isRotationZYX) + if (!t.isRotationZYX && !t.isRotationXYZ) { if (t.isRotation) { @@ -158,7 +158,17 @@ namespace sdf else if (op == "xformOp:rotateZYX" || op == "xformOp:rotateXYZ") { pxr::GfVec3f rotationEuler(0, 0, 0); - auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:rotateZYX")); + pxr::UsdAttribute attribute; + if (op == "xformOp:rotateZYX") + { + attribute = _prim.GetAttribute(pxr::TfToken("xformOp:rotateZYX")); + t.isRotationZYX = true; + } + else + { + attribute = _prim.GetAttribute(pxr::TfToken("xformOp:rotateXYZ")); + t.isRotationXYZ = true; + } if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f") { attribute.Get(&rotationEuler); @@ -182,7 +192,6 @@ namespace sdf t.q.push_back(qX); t.q.push_back(qY); t.q.push_back(qZ); - t.isRotationZYX = true; t.isRotation = true; } else if (op == "xformOp:translate") @@ -201,7 +210,6 @@ namespace sdf translate[2] = translateTmp[2]; } t.translate = ignition::math::Vector3d(translate[0], translate[1], translate[2]); - t.isTranslate = true; } else if (op == "xformOp:orient") { @@ -250,7 +258,6 @@ namespace sdf rotQuat.GetImaginary()[2] ); t.q.push_back(q); - t.isTranslate = true; t.isRotation = true; } } diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index f45f133c0..1a9584b24 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -49,9 +49,6 @@ void checkTransforms( sdf::usd::ParseUSDTransform(prim); EXPECT_EQ(_translation, usdTransforms.translate); - EXPECT_EQ( - _translation != ignition::math::Vector3d(0, 0, 0), - usdTransforms.isTranslate); EXPECT_EQ(_scale, usdTransforms.scale); EXPECT_EQ(_rotation.size(), usdTransforms.q.size()); for (unsigned int i = 0; i < _rotation.size(); ++i) @@ -125,6 +122,18 @@ TEST(Utils, GetTransform) ignition::math::Vector3d(1, 1, 1) ); + checkTransforms( + "/shapes/capsule/capsule_link/capsule_visual", + stage, + ignition::math::Vector3d(0, 0, 0), + { + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(0, 0, M_PI_2) + }, + ignition::math::Vector3d(1, 1, 1) + ); + checkTransforms( "/shapes/ellipsoid", stage, From faafcddbef51bd31efca38f4d2ff19ab1885aedb Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 14 Mar 2022 14:31:23 +0100 Subject: [PATCH 16/33] Removed WorldInterface class Signed-off-by: ahcorde --- usd/src/usd_parser/USD2SDF.cc | 21 ++----- usd/src/usd_parser/USDPhysics.cc | 16 +++-- usd/src/usd_parser/USDPhysics.hh | 5 +- usd/src/usd_parser/USDPhysics_TEST.cc | 15 ++--- usd/src/usd_parser/USDWorld.cc | 6 +- usd/src/usd_parser/USDWorld.hh | 4 +- .../usd_parser/usd_model/WorldInterface.hh | 59 ------------------- 7 files changed, 28 insertions(+), 98 deletions(-) delete mode 100644 usd/src/usd_parser/usd_model/WorldInterface.hh diff --git a/usd/src/usd_parser/USD2SDF.cc b/usd/src/usd_parser/USD2SDF.cc index 9b70cf1c8..7f6304e0b 100644 --- a/usd/src/usd_parser/USD2SDF.cc +++ b/usd/src/usd_parser/USD2SDF.cc @@ -19,7 +19,6 @@ #include "sdf/Console.hh" #include "sdf/Types.hh" -#include "usd_model/WorldInterface.hh" #include "USDWorld.hh" #include "sdf/Root.hh" @@ -34,9 +33,9 @@ UsdErrors USD2SDF::Read(const std::string &_fileName, { UsdErrors errors; - WorldInterface worldInterface; + sdf::World sdfWorld; - const auto errorsParseUSD = parseUSDWorld(_fileName, worldInterface); + const auto errorsParseUSD = parseUSDWorld(_fileName, sdfWorld); if (!errorsParseUSD.empty()) { errors.emplace_back(UsdError( @@ -45,26 +44,14 @@ UsdErrors USD2SDF::Read(const std::string &_fileName, return errors; } - sdf::World world; - - std::string worldName = worldInterface.worldName; - if (worldName.empty()) - { - world.SetName("world_name"); - } - else - { - world.SetName(worldName + "_world"); - } - auto addWorldErrors = _root.AddWorld(world); + auto addWorldErrors = _root.AddWorld(sdfWorld); if (!addWorldErrors.empty()) { errors.emplace_back(UsdError( UsdErrorCode::SDF_ERROR, - "Error adding the world [" + worldName + "]")); + "Error adding the world [" + sdfWorld.Name() + "]")); return errors; } - world.SetGravity(worldInterface.gravity * worldInterface.magnitude); return errors; } diff --git a/usd/src/usd_parser/USDPhysics.cc b/usd/src/usd_parser/USDPhysics.cc index 6e5a92098..3ef402843 100644 --- a/usd/src/usd_parser/USDPhysics.cc +++ b/usd/src/usd_parser/USDPhysics.cc @@ -23,6 +23,8 @@ #include #pragma pop_macro ("__DEPRECATED") +#include "sdf/World.hh" + namespace sdf { inline namespace SDF_VERSION_NAMESPACE { @@ -30,9 +32,11 @@ namespace usd { void ParseUSDPhysicsScene( const pxr::UsdPhysicsScene &_scene, - WorldInterface &_world, + sdf::World &_world, double _metersPerUnit) { + ignition::math::Vector3d worldGravity{0, 0, -1}; + float magnitude {9.8f}; const auto gravityAttr = _scene.GetGravityDirectionAttr(); if (gravityAttr) { @@ -42,22 +46,22 @@ namespace usd !ignition::math::equal(0.0f, gravity[1]) && !ignition::math::equal(0.0f, gravity[2])) { - _world.gravity[0] = gravity[0]; - _world.gravity[1] = gravity[1]; - _world.gravity[2] = gravity[2]; + worldGravity[0] = gravity[0]; + worldGravity[1] = gravity[1]; + worldGravity[2] = gravity[2]; } } const auto magnitudeAttr = _scene.GetGravityMagnitudeAttr(); if (magnitudeAttr) { - float magnitude; magnitudeAttr.Get(&magnitude); if (!std::isnan(magnitude) && !std::isinf(magnitude)) { - _world.magnitude = magnitude * _metersPerUnit; + magnitude = magnitude * _metersPerUnit; } } + _world.SetGravity(worldGravity * magnitude); } } } diff --git a/usd/src/usd_parser/USDPhysics.hh b/usd/src/usd_parser/USDPhysics.hh index ee6627395..e8f4f54c1 100644 --- a/usd/src/usd_parser/USDPhysics.hh +++ b/usd/src/usd_parser/USDPhysics.hh @@ -18,8 +18,6 @@ #ifndef USD_PARSER_PHYSYCS_HH #define USD_PARSER_PHYSYCS_HH -#include "usd_model/WorldInterface.hh" - #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED #include @@ -28,6 +26,7 @@ #include "sdf/config.hh" #include "sdf/usd/Export.hh" +#include "sdf/World.hh" namespace sdf { // Inline bracket to help doxygen filtering. @@ -41,7 +40,7 @@ namespace sdf /// \param[in] _metersPerUnit meters per unit in the USD void IGNITION_SDFORMAT_USD_VISIBLE ParseUSDPhysicsScene( const pxr::UsdPhysicsScene &_scene, - WorldInterface &_world, + sdf::World &_world, double _metersPerUnit); } } diff --git a/usd/src/usd_parser/USDPhysics_TEST.cc b/usd/src/usd_parser/USDPhysics_TEST.cc index 13ae67e91..df9455b88 100644 --- a/usd/src/usd_parser/USDPhysics_TEST.cc +++ b/usd/src/usd_parser/USDPhysics_TEST.cc @@ -33,7 +33,6 @@ #include "test_utils.hh" #include "USDPhysics.hh" -#include "usd_model/WorldInterface.hh" ///////////////////////////////////////////////// TEST(USDPhysicsTest, AvailablePhysics) @@ -46,14 +45,13 @@ TEST(USDPhysicsTest, AvailablePhysics) pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics"))); EXPECT_TRUE(physicsScene); - sdf::usd::WorldInterface worldInterface; + sdf::World world; const double metersPerUnit = 1.0; sdf::usd::ParseUSDPhysicsScene( - physicsScene, worldInterface, metersPerUnit); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface.gravity); - EXPECT_FLOAT_EQ(9.8f, worldInterface.magnitude); + physicsScene, world, metersPerUnit); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -9.8), world.Gravity()); } ///////////////////////////////////////////////// @@ -67,12 +65,11 @@ TEST(USDPhysicsTest, UnavailablePhysics) pxr::UsdPhysicsScene(stage->GetPrimAtPath(pxr::SdfPath("/shapes/physics"))); EXPECT_FALSE(physicsScene); - sdf::usd::WorldInterface worldInterface; + sdf::World world; const double metersPerUnit = 1.0; sdf::usd::ParseUSDPhysicsScene( - physicsScene, worldInterface, metersPerUnit); - EXPECT_EQ(ignition::math::Vector3d(0, 0, -1), worldInterface.gravity); - EXPECT_FLOAT_EQ(9.8f, worldInterface.magnitude); + physicsScene, world, metersPerUnit); + EXPECT_EQ(ignition::math::Vector3d(0, 0, -9.8), world.Gravity()); } diff --git a/usd/src/usd_parser/USDWorld.cc b/usd/src/usd_parser/USDWorld.cc index 9957aeb9c..1bf0e0e5c 100644 --- a/usd/src/usd_parser/USDWorld.cc +++ b/usd/src/usd_parser/USDWorld.cc @@ -28,13 +28,15 @@ #include "sdf/usd/usd_parser/USDStage.hh" #include "USDPhysics.hh" +#include "sdf/World.hh" + namespace sdf { inline namespace SDF_VERSION_NAMESPACE { namespace usd { UsdErrors parseUSDWorld(const std::string &_inputFileName, - WorldInterface &_world) + sdf::World &_world) { UsdErrors errors; USDData usdData(_inputFileName); @@ -49,7 +51,7 @@ namespace usd "Unable to open [" + _inputFileName + "]")); return errors; } - _world.worldName = reference->GetDefaultPrim().GetName().GetText(); + _world.SetName(reference->GetDefaultPrim().GetName().GetText()); auto range = pxr::UsdPrimRange::Stage(reference); for (auto const &prim : range) diff --git a/usd/src/usd_parser/USDWorld.hh b/usd/src/usd_parser/USDWorld.hh index fef1639a6..6574f501e 100644 --- a/usd/src/usd_parser/USDWorld.hh +++ b/usd/src/usd_parser/USDWorld.hh @@ -23,7 +23,7 @@ #include "sdf/sdf_config.h" #include "sdf/usd/UsdError.hh" -#include "usd_model/WorldInterface.hh" +#include "sdf/World.hh" namespace sdf { @@ -40,7 +40,7 @@ namespace sdf /// occurred when parsing the world information of _inputFileNameUsd UsdErrors parseUSDWorld( const std::string &_inputFileNameUsd, - WorldInterface &_world); + sdf::World &_world); } } } diff --git a/usd/src/usd_parser/usd_model/WorldInterface.hh b/usd/src/usd_parser/usd_model/WorldInterface.hh deleted file mode 100644 index 6da6f9c31..000000000 --- a/usd/src/usd_parser/usd_model/WorldInterface.hh +++ /dev/null @@ -1,59 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#ifndef USD_PARSER_USD_MODEL_WORLD_INTERFACE_HH_ -#define USD_PARSER_USD_MODEL_WORLD_INTERFACE_HH_ - -#include -#include -#include - -#include - -#include "sdf/sdf_config.h" - -namespace sdf -{ - // Inline bracket to help doxygen filtering. - inline namespace SDF_VERSION_NAMESPACE { - // - namespace usd - { - /// \brief This struct stores data about the world - struct WorldInterface { - /// \brief World name - std::string worldName; - - /// \brief Magnitude of the gravity - float magnitude {9.8f}; - - /// \brief Gravity (X, Y, Z) - ignition::math::Vector3d gravity {0.0, 0.0, -1.0}; - - friend std::ostream& operator<<( - std::ostream& os, const WorldInterface& _world) - { - os << "World name: " << _world.worldName - << ", Gravity: " << _world.gravity * _world.magnitude; - return os; - } - }; - } - } -} - -#endif From 5f754dc631a8a5f94613e8528dd92c0892f5368f Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 14 Mar 2022 15:53:16 +0100 Subject: [PATCH 17/33] fixed test Signed-off-by: ahcorde --- usd/src/usd_parser/USDWorld.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/usd/src/usd_parser/USDWorld.cc b/usd/src/usd_parser/USDWorld.cc index 1bf0e0e5c..fb525ed81 100644 --- a/usd/src/usd_parser/USDWorld.cc +++ b/usd/src/usd_parser/USDWorld.cc @@ -51,7 +51,15 @@ namespace usd "Unable to open [" + _inputFileName + "]")); return errors; } - _world.SetName(reference->GetDefaultPrim().GetName().GetText()); + std::string worldName = reference->GetDefaultPrim().GetName().GetText(); + if (!worldName.empty()) + { + _world.SetName("world_name"); + } + else + { + _world.SetName(worldName + "_world"); + } auto range = pxr::UsdPrimRange::Stage(reference); for (auto const &prim : range) From 778c017752d15afd820a898cce780173f2345c0b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Mar 2022 23:01:50 +0100 Subject: [PATCH 18/33] Added feedback Signed-off-by: ahcorde --- .../sdf/usd/usd_parser/USDTransforms.hh | 51 +- usd/src/usd_parser/USDTransforms.cc | 537 +++++++++++------- usd/src/usd_parser/USDTransforms_TEST.cc | 18 +- 3 files changed, 376 insertions(+), 230 deletions(-) diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 26c9f1633..2f071696d 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -22,8 +22,11 @@ #include #include +#include #include +#include + // TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings // are reported using #pragma message so normal diagnostic flags cannot remove // them. This workaround requires this block to be used whenever usd is @@ -48,31 +51,55 @@ namespace sdf namespace usd { /// \brief This class stores the transforms of a schema - /// This might contains scale, translate or rotation operations + /// This might contain scale, translate or rotation operations /// The booleans are used to check if there is a transform defined /// in the schema /// Rotation is splitted in a vector because this might be defined /// as a rotation of 3 angles (ZYX, XYZ, etc). - struct IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms + class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms { - /// \brief Scale of the schema - ignition::math::Vector3d scale{1, 1, 1}; + /// \brief Default constructor + public: UDSTransforms(); + + /// \brief Translate + public: ignition::math::Vector3d Translate(); + + /// \brief Scale + public: ignition::math::Vector3d Scale(); + + /// \brief Rotation + public: std::vector Rotations(); + + /// \brief Set translate + public: void SetTranslate(const ignition::math::Vector3d &_translate); - /// \brief Rotation of the schema - std::vector q; + /// \brief Set scale + public: void SetScale(const ignition::math::Vector3d &_scale); - /// \brief Translatio of the schema - ignition::math::Vector3d translate{0, 0, 0}; + /// \brief Add rotation + public: void AddRotation(const ignition::math::Quaterniond &_q); /// \brief True if there is a rotation ZYX defined or false otherwise - bool isRotationZYX = false; + bool RotationZYX(); /// \brief True if there is a rotation XYZ defined or false otherwise - bool isRotationXYZ = false; + bool RotationXYZ(); /// \brief True if there is a rotation (as a quaterion) defined /// or false otherwise - bool isRotation = false; + bool Rotation(); + + /// \brief Set if there is any rotation ZYX defined + void SetRotationZYX(bool _rotationZYX); + + /// \brief Set if there is any rotation XYZ defined + void SetRotationXYZ(bool _rotationXYZ); + + /// \brief Set if there is any rotation defined + void SetRotation(bool _rotation); + + /// \brief Private data pointer. + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \brief This function will parse all the parents transforms of a prim @@ -84,7 +111,7 @@ namespace sdf /// \param[out] _scale The scale of the prims /// \param[in] _schemaToStop Name of the prim where the loop will stop /// reading transforms - void IGNITION_SDFORMAT_USD_VISIBLE GetAllTransforms( + void GetAllTransforms( const pxr::UsdPrim &_prim, USDData &_usdData, std::vector &_tfs, diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index d26ca9a24..d42a3948f 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -23,246 +23,373 @@ namespace sdf { - // Inline bracke to help doxygen filtering. - inline namespace SDF_VERSION_NAMESPACE { - // - namespace usd - { +// Inline bracke to help doxygen filtering. +inline namespace SDF_VERSION_NAMESPACE { +// +namespace usd +{ - void GetAllTransforms( - const pxr::UsdPrim &_prim, - USDData &_usdData, - std::vector &_tfs, - ignition::math::Vector3d &_scale, - const std::string &_schemaToStop) - { - pxr::UsdPrim parent = _prim; - double metersPerUnit = 1.0; - std::string upAxis = "Y"; - - // this assumes that there can only be one stage - auto stageData = _usdData.FindStage(parent.GetPath().GetName()); - if (stageData.second != nullptr) { - metersPerUnit = stageData.second->MetersPerUnit(); - upAxis = stageData.second->UpAxis(); - } +const std::string kXFormOpTranslate = {"xformOp:translate"}; +const std::string kXFormOpOrient = {"xformOp:orient"}; +const std::string kXFormOpTransform = {"xformOp:transform"}; +const std::string kXFormOpScale = {"xformOp:scale"}; +const std::string kXFormOpRotateXYZ = {"xformOp:rotateXYZ"}; +const std::string kXFormOpRotateZYX = {"xformOp:rotateZYX"}; + +const std::string kGfVec3fString = {"GfVec3f"}; +const std::string kGfVec3dString = {"GfVec3d"}; +const std::string kGfQuatfString = {"GfQuatf"}; +const std::string kGfQuatdString = {"GfQuatd"}; + +/// \brief Private altimeter data. +class UDSTransforms::Implementation +{ + /// \brief Scale of the schema + public: ignition::math::Vector3d scale{1, 1, 1}; + + /// \brief Rotation of the schema + public: std::vector q; + + /// \brief Translatio of the schema + public: ignition::math::Vector3d translate{0, 0, 0}; + + /// \brief True if there is a rotation ZYX defined or false otherwise + public: bool isRotationZYX = false; + + /// \brief True if there is a rotation XYZ defined or false otherwise + public: bool isRotationXYZ = false; + + /// \brief True if there is a rotation (as a quaterion) defined + /// or false otherwise + public: bool isRotation = false; +}; + +///////////////////////////////////////////////// +UDSTransforms::UDSTransforms() + : dataPtr(ignition::utils::MakeImpl()) +{ +} + +////////////////////////////////////////////////// +ignition::math::Vector3d UDSTransforms::Translate() +{ + return this->dataPtr->translate; +} + +////////////////////////////////////////////////// +ignition::math::Vector3d UDSTransforms::Scale() +{ + return this->dataPtr->scale; +} + +////////////////////////////////////////////////// +std::vector + UDSTransforms::Rotations() +{ + return this->dataPtr->q; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetTranslate( + const ignition::math::Vector3d &_translate) +{ + this->dataPtr->translate = _translate; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetScale( + const ignition::math::Vector3d &_scale) +{ + this->dataPtr->scale = _scale; +} + +////////////////////////////////////////////////// +void UDSTransforms::AddRotation( + const ignition::math::Quaterniond &_q) +{ + this->dataPtr->q.push_back(_q); +} + +////////////////////////////////////////////////// +bool UDSTransforms::RotationZYX() +{ + return this->dataPtr->isRotationZYX; +} + +////////////////////////////////////////////////// +bool UDSTransforms::RotationXYZ() +{ + return this->dataPtr->isRotationXYZ; +} + +////////////////////////////////////////////////// +bool UDSTransforms::Rotation() +{ + return this->dataPtr->isRotation; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetRotationZYX(bool _rotationZYX) +{ + this->dataPtr->isRotationZYX = _rotationZYX; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetRotationXYZ(bool _rotationXYZ) +{ + this->dataPtr->isRotationXYZ = _rotationXYZ; +} + +////////////////////////////////////////////////// +void UDSTransforms::SetRotation(bool _rotation) +{ + this->dataPtr->isRotation = _rotation; +} + +////////////////////////////////////////////////// +void GetAllTransforms( + const pxr::UsdPrim &_prim, + USDData &_usdData, + std::vector &_tfs, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop) +{ + pxr::UsdPrim parent = _prim; + double metersPerUnit = 1.0; + std::string upAxis = "Y"; - while (parent) + // this assumes that there can only be one stage + auto stageData = _usdData.FindStage(parent.GetPath().GetName()); + if (stageData.second != nullptr) { + metersPerUnit = stageData.second->MetersPerUnit(); + upAxis = stageData.second->UpAxis(); + } + + while (parent) + { + if (pxr::TfStringify(parent.GetPath()) == _schemaToStop) { - if (pxr::TfStringify(parent.GetPath()) == _schemaToStop) - { - return; - } + return; + } - UDSTransforms t = ParseUSDTransform(parent); + UDSTransforms t = ParseUSDTransform(parent); - ignition::math::Pose3d pose; - _scale *= t.scale; + ignition::math::Pose3d pose; + _scale *= t.Scale(); - pose.Pos() = t.translate * metersPerUnit; - // scaling is lost when we convert to pose, so we pre-scale the translation - // to make them match the scaled values. - if (!_tfs.empty()) { - auto& child = _tfs.back(); - child.Pos().Set( - child.Pos().X() * t.scale[0], - child.Pos().Y() * t.scale[1], - child.Pos().Z() * t.scale[2]); - } + pose.Pos() = t.Translate() * metersPerUnit; + // scaling is lost when we convert to pose, so we pre-scale the + // translation to make them match the scaled values. + if (!_tfs.empty()) { + auto& child = _tfs.back(); + child.Pos().Set( + child.Pos().X() * t.Scale()[0], + child.Pos().Y() * t.Scale()[1], + child.Pos().Z() * t.Scale()[2]); + } - if (!t.isRotationZYX && !t.isRotationXYZ) - { - if (t.isRotation) - { - pose.Rot() = t.q[0]; - } - _tfs.push_back(pose); - } - else + if (!t.RotationZYX() && !t.RotationXYZ()) + { + if (t.Rotation()) { - ignition::math::Pose3d poseZ = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), t.q[2]); - ignition::math::Pose3d poseY = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), t.q[1]); - ignition::math::Pose3d poseX = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), t.q[0]); - - ignition::math::Pose3d poseT = ignition::math::Pose3d( - t.translate * metersPerUnit, - ignition::math::Quaterniond(1, 0, 0, 0)); - - _tfs.push_back(poseZ); - _tfs.push_back(poseY); - _tfs.push_back(poseX); - _tfs.push_back(poseT); + pose.Rot() = t.Rotations()[0]; } - parent = parent.GetParent(); + _tfs.push_back(pose); } - - if (upAxis == "Y") + else { - ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), - ignition::math::Quaterniond(IGN_PI_2, 0, 0)); - _tfs.push_back(poseUpAxis); + ignition::math::Pose3d poseZ = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), t.Rotations()[2]); + ignition::math::Pose3d poseY = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), t.Rotations()[1]); + ignition::math::Pose3d poseX = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), t.Rotations()[0]); + + ignition::math::Pose3d poseT = ignition::math::Pose3d( + t.Translate() * metersPerUnit, + ignition::math::Quaterniond(1, 0, 0, 0)); + + _tfs.push_back(poseZ); + _tfs.push_back(poseY); + _tfs.push_back(poseX); + _tfs.push_back(poseT); } + parent = parent.GetParent(); } - void GetTransform( - const pxr::UsdPrim &_prim, - USDData &_usdData, - ignition::math::Pose3d &_pose, - ignition::math::Vector3d &_scale, - const std::string &_schemaToStop) + if (upAxis == "Y") { - std::vector tfs; - GetAllTransforms(_prim, _usdData, tfs, _scale, _schemaToStop); - for (auto & rt : tfs) - { - _pose = rt * _pose; - } + ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( + ignition::math::Vector3d(0 ,0 ,0), + ignition::math::Quaterniond(IGN_PI_2, 0, 0)); + _tfs.push_back(poseUpAxis); } +} - UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) +////////////////////////////////////////////////// +void GetTransform( + const pxr::UsdPrim &_prim, + USDData &_usdData, + ignition::math::Pose3d &_pose, + ignition::math::Vector3d &_scale, + const std::string &_schemaToStop) +{ + std::vector tfs; + GetAllTransforms(_prim, _usdData, tfs, _scale, _schemaToStop); + for (auto & rt : tfs) { - auto variant_geom = pxr::UsdGeomGprim(_prim); - auto transforms = variant_geom.GetXformOpOrderAttr(); + _pose = rt * _pose; + } +} + +////////////////////////////////////////////////// +UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) +{ + auto variantGeom = pxr::UsdGeomGprim(_prim); + auto transforms = variantGeom.GetXformOpOrderAttr(); - pxr::GfVec3f scale(1, 1, 1); - pxr::GfVec3f translate(0, 0, 0); - pxr::GfQuatf rotationQuad(1, 0, 0, 0); + pxr::GfVec3f scale(1, 1, 1); + pxr::GfVec3f translate(0, 0, 0); + pxr::GfQuatf rotationQuad(1, 0, 0, 0); - UDSTransforms t; + UDSTransforms t; - pxr::VtTokenArray xformOpOrder; - transforms.Get(&xformOpOrder); - for (auto & op: xformOpOrder) + pxr::VtTokenArray xformOpOrder; + transforms.Get(&xformOpOrder); + for (auto & op: xformOpOrder) + { + if (op == kXFormOpScale) { - if (op == "xformOp:scale") + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpScale)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) { - auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:scale")); - if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f") - { - attribute.Get(&scale); - } - else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d") - { - pxr::GfVec3d scaleTmp(1, 1, 1); - attribute.Get(&scaleTmp); - scale[0] = scaleTmp[0]; - scale[1] = scaleTmp[1]; - scale[2] = scaleTmp[2]; - } - t.scale = ignition::math::Vector3d(scale[0], scale[1], scale[2]); + attribute.Get(&scale); } - else if (op == "xformOp:rotateZYX" || op == "xformOp:rotateXYZ") + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) { - pxr::GfVec3f rotationEuler(0, 0, 0); - pxr::UsdAttribute attribute; - if (op == "xformOp:rotateZYX") - { - attribute = _prim.GetAttribute(pxr::TfToken("xformOp:rotateZYX")); - t.isRotationZYX = true; - } - else - { - attribute = _prim.GetAttribute(pxr::TfToken("xformOp:rotateXYZ")); - t.isRotationXYZ = true; - } - if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f") - { - attribute.Get(&rotationEuler); - } - else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d") - { - pxr::GfVec3d rotationEulerTmp(0, 0, 0); - attribute.Get(&rotationEulerTmp); - rotationEuler[0] = rotationEulerTmp[0]; - rotationEuler[1] = rotationEulerTmp[1]; - rotationEuler[2] = rotationEulerTmp[2]; - } - ignition::math::Quaterniond qX, qY, qZ; - ignition::math::Angle angleX(rotationEuler[0] * IGN_PI / 180.0); - ignition::math::Angle angleY(rotationEuler[1] * IGN_PI / 180.0); - ignition::math::Angle angleZ(rotationEuler[2] * IGN_PI / 180.0); - qX = ignition::math::Quaterniond(angleX.Normalized().Radian(), 0, 0); - qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0); - qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian()); - - t.q.push_back(qX); - t.q.push_back(qY); - t.q.push_back(qZ); - t.isRotation = true; + pxr::GfVec3d scaleTmp(1, 1, 1); + attribute.Get(&scaleTmp); + scale[0] = scaleTmp[0]; + scale[1] = scaleTmp[1]; + scale[2] = scaleTmp[2]; } - else if (op == "xformOp:translate") + t.SetScale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); + } + else if (op == kXFormOpRotateZYX || op == kXFormOpRotateXYZ) + { + pxr::GfVec3f rotationEuler(0, 0, 0); + pxr::UsdAttribute attribute; + if (op == kXFormOpRotateZYX) + { + attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateZYX)); + t.SetRotationZYX(true); + } + else { - auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:translate")); - if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3f") - { - attribute.Get(&translate); - } - else if (attribute.GetTypeName().GetCPPTypeName() == "GfVec3d") - { - pxr::GfVec3d translateTmp(0, 0, 0); - attribute.Get(&translateTmp); - translate[0] = translateTmp[0]; - translate[1] = translateTmp[1]; - translate[2] = translateTmp[2]; - } - t.translate = ignition::math::Vector3d(translate[0], translate[1], translate[2]); + attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateXYZ)); + t.SetRotationXYZ(true); } - else if (op == "xformOp:orient") + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) { - auto attribute = _prim.GetAttribute(pxr::TfToken("xformOp:orient")); - if (attribute.GetTypeName().GetCPPTypeName() == "GfQuatf") - { - attribute.Get(&rotationQuad); - } - else if (attribute.GetTypeName().GetCPPTypeName() == "GfQuatd") - { - pxr::GfQuatd rotationQuadTmp; - attribute.Get(&rotationQuadTmp); - rotationQuad.SetImaginary( - rotationQuadTmp.GetImaginary()[0], - rotationQuadTmp.GetImaginary()[1], - rotationQuadTmp.GetImaginary()[2]); - rotationQuad.SetReal(rotationQuadTmp.GetReal()); - } - ignition::math::Quaterniond q( - rotationQuad.GetReal(), - rotationQuad.GetImaginary()[0], - rotationQuad.GetImaginary()[1], - rotationQuad.GetImaginary()[2]); - t.q.push_back(q); - t.isRotation = true; + attribute.Get(&rotationEuler); } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) + { + pxr::GfVec3d rotationEulerTmp(0, 0, 0); + attribute.Get(&rotationEulerTmp); + rotationEuler[0] = rotationEulerTmp[0]; + rotationEuler[1] = rotationEulerTmp[1]; + rotationEuler[2] = rotationEulerTmp[2]; + } + ignition::math::Quaterniond qX, qY, qZ; + ignition::math::Angle angleX(IGN_DTOR(rotationEuler[0])); + ignition::math::Angle angleY(IGN_DTOR(rotationEuler[1])); + ignition::math::Angle angleZ(IGN_DTOR(rotationEuler[2])); + qX = ignition::math::Quaterniond(angleX.Normalized().Radian(), 0, 0); + qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0); + qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian()); - if (op == "xformOp:transform") + t.AddRotation(qX); + t.AddRotation(qY); + t.AddRotation(qZ); + t.SetRotation(true); + } + else if (op == kXFormOpTranslate) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpTranslate)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) + { + attribute.Get(&translate); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3dString) { - // FIXME: Shear is lost (does sdformat support it?). - pxr::GfMatrix4d transform; - _prim.GetAttribute(pxr::TfToken("xformOp:transform")).Get(&transform); - const auto rot = transform.RemoveScaleShear(); - const auto scaleShear = transform * rot.GetInverse(); - - t.scale[0] = scaleShear[0][0]; - t.scale[1] = scaleShear[1][1]; - t.scale[2] = scaleShear[2][2]; - - const auto rotQuat = rot.ExtractRotationQuat(); - t.translate = ignition::math::Vector3d(transform[3][0], transform[3][1], transform[3][2]); - ignition::math::Quaterniond q( - rotQuat.GetReal(), - rotQuat.GetImaginary()[0], - rotQuat.GetImaginary()[1], - rotQuat.GetImaginary()[2] - ); - t.q.push_back(q); - t.isRotation = true; + pxr::GfVec3d translateTmp(0, 0, 0); + attribute.Get(&translateTmp); + translate[0] = translateTmp[0]; + translate[1] = translateTmp[1]; + translate[2] = translateTmp[2]; } + t.SetTranslate(ignition::math::Vector3d( + translate[0], + translate[1], + translate[2])); + } + else if (op == kXFormOpOrient) + { + auto attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpOrient)); + if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatfString) + { + attribute.Get(&rotationQuad); + } + else if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatfString) + { + pxr::GfQuatd rotationQuadTmp; + attribute.Get(&rotationQuadTmp); + rotationQuad.SetImaginary( + rotationQuadTmp.GetImaginary()[0], + rotationQuadTmp.GetImaginary()[1], + rotationQuadTmp.GetImaginary()[2]); + rotationQuad.SetReal(rotationQuadTmp.GetReal()); + } + ignition::math::Quaterniond q( + rotationQuad.GetReal(), + rotationQuad.GetImaginary()[0], + rotationQuad.GetImaginary()[1], + rotationQuad.GetImaginary()[2]); + t.AddRotation(q); + t.SetRotation(true); + } + + if (op == kXFormOpTransform) + { + // FIXME: Shear is lost (does sdformat support it?). + pxr::GfMatrix4d transform; + _prim.GetAttribute(pxr::TfToken(kXFormOpTransform)).Get(&transform); + const auto rot = transform.RemoveScaleShear(); + const auto scaleShear = transform * rot.GetInverse(); + + t.SetScale(ignition::math::Vector3d( + scaleShear[0][0], + scaleShear[1][1], + scaleShear[2][2])); + + const auto rotQuat = rot.ExtractRotationQuat(); + t.SetTranslate(ignition::math::Vector3d( + transform[3][0], + transform[3][1], + transform[3][2])); + ignition::math::Quaterniond q( + rotQuat.GetReal(), + rotQuat.GetImaginary()[0], + rotQuat.GetImaginary()[1], + rotQuat.GetImaginary()[2] + ); + t.AddRotation(q); + t.SetRotation(true); } - return t; } + return t; +} } } } diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index 1a9584b24..2d06ee82f 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -48,15 +48,15 @@ void checkTransforms( sdf::usd::UDSTransforms usdTransforms = sdf::usd::ParseUSDTransform(prim); - EXPECT_EQ(_translation, usdTransforms.translate); - EXPECT_EQ(_scale, usdTransforms.scale); - EXPECT_EQ(_rotation.size(), usdTransforms.q.size()); + EXPECT_EQ(_translation, usdTransforms.Translate()); + EXPECT_EQ(_scale, usdTransforms.Scale()); + EXPECT_EQ(_rotation.size(), usdTransforms.Rotations().size()); for (unsigned int i = 0; i < _rotation.size(); ++i) { - EXPECT_EQ(_rotation[i], usdTransforms.q[i]); + EXPECT_EQ(_rotation[i], usdTransforms.Rotations()[i]); } - EXPECT_EQ(!_rotation.empty(), usdTransforms.isRotation); + EXPECT_EQ(!_rotation.empty(), usdTransforms.Rotation()); } ///////////////////////////////////////////////// @@ -193,11 +193,3 @@ TEST(Utils, GetAllTransform) ignition::math::Quaterniond(1, 0, 0, 0)), pose); } - -///////////////////////////////////////////////// -/// Main -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 4b20b929e1e14c66e6a596b2863cd2443de2f22a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Mar 2022 10:03:20 +0100 Subject: [PATCH 19/33] make linters happy Signed-off-by: ahcorde --- .../sdf/usd/usd_parser/USDTransforms.hh | 2 +- usd/src/usd_parser/USDTransforms.cc | 32 +++++++++---------- usd/src/usd_parser/USDTransforms_TEST.cc | 2 +- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 2f071696d..2b02d0e99 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -27,7 +27,7 @@ #include -// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings // are reported using #pragma message so normal diagnostic flags cannot remove // them. This workaround requires this block to be used whenever usd is // included. diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index d42a3948f..1767e927d 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -29,17 +29,17 @@ inline namespace SDF_VERSION_NAMESPACE { namespace usd { -const std::string kXFormOpTranslate = {"xformOp:translate"}; -const std::string kXFormOpOrient = {"xformOp:orient"}; -const std::string kXFormOpTransform = {"xformOp:transform"}; -const std::string kXFormOpScale = {"xformOp:scale"}; -const std::string kXFormOpRotateXYZ = {"xformOp:rotateXYZ"}; -const std::string kXFormOpRotateZYX = {"xformOp:rotateZYX"}; - -const std::string kGfVec3fString = {"GfVec3f"}; -const std::string kGfVec3dString = {"GfVec3d"}; -const std::string kGfQuatfString = {"GfQuatf"}; -const std::string kGfQuatdString = {"GfQuatd"}; +const char kXFormOpTranslate[] = {"xformOp:translate"}; +const char kXFormOpOrient[] = {"xformOp:orient"}; +const char kXFormOpTransform[] = {"xformOp:transform"}; +const char kXFormOpScale[] = {"xformOp:scale"}; +const char kXFormOpRotateXYZ[] = {"xformOp:rotateXYZ"}; +const char kXFormOpRotateZYX[] = {"xformOp:rotateZYX"}; + +const char kGfVec3fString[] = {"GfVec3f"}; +const char kGfVec3dString[] = {"GfVec3d"}; +const char kGfQuatfString[] = {"GfQuatf"}; +const char kGfQuatdString[] = {"GfQuatd"}; /// \brief Private altimeter data. class UDSTransforms::Implementation @@ -199,11 +199,11 @@ void GetAllTransforms( else { ignition::math::Pose3d poseZ = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), t.Rotations()[2]); + ignition::math::Vector3d(0, 0 ,0), t.Rotations()[2]); ignition::math::Pose3d poseY = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), t.Rotations()[1]); + ignition::math::Vector3d(0, 0 ,0), t.Rotations()[1]); ignition::math::Pose3d poseX = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), t.Rotations()[0]); + ignition::math::Vector3d(0, 0 ,0), t.Rotations()[0]); ignition::math::Pose3d poseT = ignition::math::Pose3d( t.Translate() * metersPerUnit, @@ -220,7 +220,7 @@ void GetAllTransforms( if (upAxis == "Y") { ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( - ignition::math::Vector3d(0 ,0 ,0), + ignition::math::Vector3d(0, 0 ,0), ignition::math::Quaterniond(IGN_PI_2, 0, 0)); _tfs.push_back(poseUpAxis); } @@ -256,7 +256,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) pxr::VtTokenArray xformOpOrder; transforms.Get(&xformOpOrder); - for (auto & op: xformOpOrder) + for (auto & op : xformOpOrder) { if (op == kXFormOpScale) { diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index 2d06ee82f..3923d7955 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -24,7 +24,7 @@ #include "test_config.h" #include "test_utils.hh" -// TODO(ahcorde):this is to remove deprecated "warnings" in usd, these warnings +// TODO(ahcorde) this is to remove deprecated "warnings" in usd, these warnings // are reported using #pragma message so normal diagnostic flags cannot remove // them. This workaround requires this block to be used whenever usd is // included. From 9b5536478e8071d7ce6ff6f36e6141bd21351618 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Mar 2022 18:23:59 +0100 Subject: [PATCH 20/33] make linters happy Signed-off-by: ahcorde --- usd/src/usd_parser/USDTransforms.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index 1767e927d..5b436d17c 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -199,11 +199,11 @@ void GetAllTransforms( else { ignition::math::Pose3d poseZ = ignition::math::Pose3d( - ignition::math::Vector3d(0, 0 ,0), t.Rotations()[2]); + ignition::math::Vector3d(0, 0, 0), t.Rotations()[2]); ignition::math::Pose3d poseY = ignition::math::Pose3d( - ignition::math::Vector3d(0, 0 ,0), t.Rotations()[1]); + ignition::math::Vector3d(0, 0, 0), t.Rotations()[1]); ignition::math::Pose3d poseX = ignition::math::Pose3d( - ignition::math::Vector3d(0, 0 ,0), t.Rotations()[0]); + ignition::math::Vector3d(0, 0, 0), t.Rotations()[0]); ignition::math::Pose3d poseT = ignition::math::Pose3d( t.Translate() * metersPerUnit, @@ -220,7 +220,7 @@ void GetAllTransforms( if (upAxis == "Y") { ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( - ignition::math::Vector3d(0, 0 ,0), + ignition::math::Vector3d(0, 0, 0), ignition::math::Quaterniond(IGN_PI_2, 0, 0)); _tfs.push_back(poseUpAxis); } From 67aae3223e9d8703cf1f58faa1573593a925e0c8 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 23 Mar 2022 12:39:09 +0100 Subject: [PATCH 21/33] Added feedback Signed-off-by: ahcorde --- usd/include/sdf/usd/usd_parser/USDData.hh | 2 +- .../sdf/usd/usd_parser/USDTransforms.hh | 59 +++++++++++-------- usd/src/usd_parser/USDData.cc | 2 +- usd/src/usd_parser/USDTransforms.cc | 48 +++++++-------- 4 files changed, 62 insertions(+), 49 deletions(-) diff --git a/usd/include/sdf/usd/usd_parser/USDData.hh b/usd/include/sdf/usd/usd_parser/USDData.hh index d76e861dd..9936858cb 100644 --- a/usd/include/sdf/usd/usd_parser/USDData.hh +++ b/usd/include/sdf/usd/usd_parser/USDData.hh @@ -82,7 +82,7 @@ namespace sdf /// \param[in] _name Name of the path to find /// \return A pair with the name of the stage and the data public: const std::pair> - FindStage(const std::string &_name); + FindStage(const std::string &_name) const; public: friend std::ostream& operator<<( std::ostream& os, const USDData& data) diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 2b02d0e99..53e0aaf3c 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -15,8 +15,8 @@ * */ -#ifndef SDF_USD_USD_PARSER_UTILS_HH_ -#define SDF_USD_USD_PARSER_UTILS_HH_ +#ifndef SDF_USD_USD_PARSER_TRANSFORMS_HH_ +#define SDF_USD_USD_PARSER_TRANSFORMS_HH_ #include #include @@ -40,12 +40,11 @@ #include "sdf/system_util.hh" #include "sdf/usd/Export.hh" #include "sdf/usd/UsdError.hh" - #include "USDData.hh" namespace sdf { - // Inline bracke to help doxygen filtering. + // Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // namespace usd @@ -62,48 +61,62 @@ namespace sdf public: UDSTransforms(); /// \brief Translate - public: ignition::math::Vector3d Translate(); + /// \return A 3D vector with the translation + public: const ignition::math::Vector3d Translate() const; /// \brief Scale - public: ignition::math::Vector3d Scale(); + /// \return A 3D vector with the scale + public: const ignition::math::Vector3d Scale() const; /// \brief Rotation - public: std::vector Rotations(); + /// \return Return a vector with all the rotations + public: const std::vector Rotations() const; /// \brief Set translate - public: void SetTranslate(const ignition::math::Vector3d &_translate); + /// \param[in] _translate Translate to set + public: void Translate(const ignition::math::Vector3d &_translate); /// \brief Set scale - public: void SetScale(const ignition::math::Vector3d &_scale); + /// \param[in] _scale Scale to set + public: void Scale(const ignition::math::Vector3d &_scale); /// \brief Add rotation + /// \param[in] _q Add quaterion to the rotation vector public: void AddRotation(const ignition::math::Quaterniond &_q); /// \brief True if there is a rotation ZYX defined or false otherwise - bool RotationZYX(); + public: bool RotationZYX() const; /// \brief True if there is a rotation XYZ defined or false otherwise - bool RotationXYZ(); + public: bool RotationXYZ() const; /// \brief True if there is a rotation (as a quaterion) defined /// or false otherwise - bool Rotation(); + public: bool Rotation() const; /// \brief Set if there is any rotation ZYX defined - void SetRotationZYX(bool _rotationZYX); + /// RotationZYX is used to determine the order of stored rotations + /// If rotationZYX is true then Rotation should be True too + /// If Rotation is false then rotationZYX could no be true + /// \param[in] _rotationZYX Set if the rotation is ZYX + public: void RotationZYX(bool _rotationZYX); /// \brief Set if there is any rotation XYZ defined - void SetRotationXYZ(bool _rotationXYZ); + /// RotationXYZ is used to determine the order of stored rotations + /// If rotationXYZ is true then Rotation should be True too + /// If Rotation is false then rotationXYZ could no be true + /// \param[in] _rotationXYZ Set if the rotation is XYZ + public: void RotationXYZ(bool _rotationXYZ); /// \brief Set if there is any rotation defined - void SetRotation(bool _rotation); + /// \param[in] _rotation Set if there is any rotation + public: void Rotation(bool _rotation); /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; - /// \brief This function will parse all the parents transforms of a prim - /// This will stop when the name of the parent is the same as _schemaToStop + /// \brief This function will parse all the parent transforms of a prim. /// \param[in] _prim Initial prim to read the transform /// \param[in] _usdData USDData structure to get info about the prim, for /// example: metersperunit @@ -113,24 +126,23 @@ namespace sdf /// reading transforms void GetAllTransforms( const pxr::UsdPrim &_prim, - USDData &_usdData, + const USDData &_usdData, std::vector &_tfs, ignition::math::Vector3d &_scale, const std::string &_schemaToStop); - /// \brief This function get the transform from a prim to the specified - /// schemaToStop variable - /// This will stop when the name of the parent is the same as _schemaToStop + /// \brief This function gets the transform from a prim to the specified + /// _schemaToStop variable /// \param[in] _prim Initial prim to read the transform /// \param[in] _usdData USDData structure to get info about the prim, for /// example: metersperunit - /// \param[out] _pose Pose of the prim + /// \param[out] _pose Pose of the prim. From _prim to _schemaToStop. /// \param[out] _scale The scale of the prim /// \param[in] _schemaToStop Name of the prim where the loop will stop /// reading transforms void IGNITION_SDFORMAT_USD_VISIBLE GetTransform( const pxr::UsdPrim &_prim, - USDData &_usdData, + const USDData &_usdData, ignition::math::Pose3d &_pose, ignition::math::Vector3d &_scale, const std::string &_schemaToStop); @@ -138,6 +150,7 @@ namespace sdf /// \brief Read the usd prim transforms. Scale, rotation or transform might /// be defined as float or doubles /// \param[in] _prim Prim where the transforms are read + /// \return A USDTransforms class with all the transform related to the prim UDSTransforms IGNITION_SDFORMAT_USD_VISIBLE ParseUSDTransform( const pxr::UsdPrim &_prim); } diff --git a/usd/src/usd_parser/USDData.cc b/usd/src/usd_parser/USDData.cc index 535b24c44..e0dcbbc84 100644 --- a/usd/src/usd_parser/USDData.cc +++ b/usd/src/usd_parser/USDData.cc @@ -236,7 +236,7 @@ namespace usd { ///////////////////////////////////////////////// const std::pair> - USDData::FindStage(const std::string &_name) + USDData::FindStage(const std::string &_name) const { for (auto &ref : this->dataPtr->references) { diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index 5b436d17c..c91a26ff6 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -71,33 +71,33 @@ UDSTransforms::UDSTransforms() } ////////////////////////////////////////////////// -ignition::math::Vector3d UDSTransforms::Translate() +const ignition::math::Vector3d UDSTransforms::Translate() const { return this->dataPtr->translate; } ////////////////////////////////////////////////// -ignition::math::Vector3d UDSTransforms::Scale() +const ignition::math::Vector3d UDSTransforms::Scale() const { return this->dataPtr->scale; } ////////////////////////////////////////////////// -std::vector - UDSTransforms::Rotations() +const std::vector + UDSTransforms::Rotations() const { return this->dataPtr->q; } ////////////////////////////////////////////////// -void UDSTransforms::SetTranslate( +void UDSTransforms::Translate( const ignition::math::Vector3d &_translate) { this->dataPtr->translate = _translate; } ////////////////////////////////////////////////// -void UDSTransforms::SetScale( +void UDSTransforms::Scale( const ignition::math::Vector3d &_scale) { this->dataPtr->scale = _scale; @@ -111,37 +111,37 @@ void UDSTransforms::AddRotation( } ////////////////////////////////////////////////// -bool UDSTransforms::RotationZYX() +bool UDSTransforms::RotationZYX() const { return this->dataPtr->isRotationZYX; } ////////////////////////////////////////////////// -bool UDSTransforms::RotationXYZ() +bool UDSTransforms::RotationXYZ() const { return this->dataPtr->isRotationXYZ; } ////////////////////////////////////////////////// -bool UDSTransforms::Rotation() +bool UDSTransforms::Rotation() const { return this->dataPtr->isRotation; } ////////////////////////////////////////////////// -void UDSTransforms::SetRotationZYX(bool _rotationZYX) +void UDSTransforms::RotationZYX(bool _rotationZYX) { this->dataPtr->isRotationZYX = _rotationZYX; } ////////////////////////////////////////////////// -void UDSTransforms::SetRotationXYZ(bool _rotationXYZ) +void UDSTransforms::RotationXYZ(bool _rotationXYZ) { this->dataPtr->isRotationXYZ = _rotationXYZ; } ////////////////////////////////////////////////// -void UDSTransforms::SetRotation(bool _rotation) +void UDSTransforms::Rotation(bool _rotation) { this->dataPtr->isRotation = _rotation; } @@ -149,7 +149,7 @@ void UDSTransforms::SetRotation(bool _rotation) ////////////////////////////////////////////////// void GetAllTransforms( const pxr::UsdPrim &_prim, - USDData &_usdData, + const USDData &_usdData, std::vector &_tfs, ignition::math::Vector3d &_scale, const std::string &_schemaToStop) @@ -159,7 +159,7 @@ void GetAllTransforms( std::string upAxis = "Y"; // this assumes that there can only be one stage - auto stageData = _usdData.FindStage(parent.GetPath().GetName()); + const auto stageData = _usdData.FindStage(parent.GetPath().GetName()); if (stageData.second != nullptr) { metersPerUnit = stageData.second->MetersPerUnit(); upAxis = stageData.second->UpAxis(); @@ -229,7 +229,7 @@ void GetAllTransforms( ////////////////////////////////////////////////// void GetTransform( const pxr::UsdPrim &_prim, - USDData &_usdData, + const USDData &_usdData, ignition::math::Pose3d &_pose, ignition::math::Vector3d &_scale, const std::string &_schemaToStop) @@ -273,7 +273,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) scale[1] = scaleTmp[1]; scale[2] = scaleTmp[2]; } - t.SetScale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); + t.Scale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); } else if (op == kXFormOpRotateZYX || op == kXFormOpRotateXYZ) { @@ -282,12 +282,12 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) if (op == kXFormOpRotateZYX) { attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateZYX)); - t.SetRotationZYX(true); + t.RotationZYX(true); } else { attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateXYZ)); - t.SetRotationXYZ(true); + t.RotationXYZ(true); } if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) { @@ -312,7 +312,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) t.AddRotation(qX); t.AddRotation(qY); t.AddRotation(qZ); - t.SetRotation(true); + t.Rotation(true); } else if (op == kXFormOpTranslate) { @@ -329,7 +329,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) translate[1] = translateTmp[1]; translate[2] = translateTmp[2]; } - t.SetTranslate(ignition::math::Vector3d( + t.Translate(ignition::math::Vector3d( translate[0], translate[1], translate[2])); @@ -357,7 +357,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) rotationQuad.GetImaginary()[1], rotationQuad.GetImaginary()[2]); t.AddRotation(q); - t.SetRotation(true); + t.Rotation(true); } if (op == kXFormOpTransform) @@ -368,13 +368,13 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) const auto rot = transform.RemoveScaleShear(); const auto scaleShear = transform * rot.GetInverse(); - t.SetScale(ignition::math::Vector3d( + t.Scale(ignition::math::Vector3d( scaleShear[0][0], scaleShear[1][1], scaleShear[2][2])); const auto rotQuat = rot.ExtractRotationQuat(); - t.SetTranslate(ignition::math::Vector3d( + t.Translate(ignition::math::Vector3d( transform[3][0], transform[3][1], transform[3][2])); @@ -385,7 +385,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) rotQuat.GetImaginary()[2] ); t.AddRotation(q); - t.SetRotation(true); + t.Rotation(true); } } return t; From bd9e026aedef53c58ce54c4df10c6f64d8c5fa01 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 23 Mar 2022 13:53:24 +0100 Subject: [PATCH 22/33] make linters happy Signed-off-by: ahcorde --- usd/include/sdf/usd/usd_parser/USDTransforms.hh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 53e0aaf3c..6c55f1d71 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -150,7 +150,8 @@ namespace sdf /// \brief Read the usd prim transforms. Scale, rotation or transform might /// be defined as float or doubles /// \param[in] _prim Prim where the transforms are read - /// \return A USDTransforms class with all the transform related to the prim + /// \return A USDTransforms class with all the transforms related to + /// the prim UDSTransforms IGNITION_SDFORMAT_USD_VISIBLE ParseUSDTransform( const pxr::UsdPrim &_prim); } From f2f58a5bebc06e477c3693929012a5175ea94905 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 24 Mar 2022 12:24:07 +0100 Subject: [PATCH 23/33] Added feedback Signed-off-by: ahcorde --- usd/include/sdf/usd/usd_parser/USDData.hh | 2 +- .../sdf/usd/usd_parser/USDTransforms.hh | 44 +++++++--------- usd/src/CMakeLists.txt | 5 ++ usd/src/usd_parser/USDData.cc | 2 +- usd/src/usd_parser/USDTransforms.cc | 50 ++++++++++++------- usd/src/usd_parser/USDTransforms_TEST.cc | 7 ++- 6 files changed, 62 insertions(+), 48 deletions(-) diff --git a/usd/include/sdf/usd/usd_parser/USDData.hh b/usd/include/sdf/usd/usd_parser/USDData.hh index 9936858cb..d76e861dd 100644 --- a/usd/include/sdf/usd/usd_parser/USDData.hh +++ b/usd/include/sdf/usd/usd_parser/USDData.hh @@ -82,7 +82,7 @@ namespace sdf /// \param[in] _name Name of the path to find /// \return A pair with the name of the stage and the data public: const std::pair> - FindStage(const std::string &_name) const; + FindStage(const std::string &_name); public: friend std::ostream& operator<<( std::ostream& os, const USDData& data) diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 6c55f1d71..04664e70c 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -15,8 +15,8 @@ * */ -#ifndef SDF_USD_USD_PARSER_TRANSFORMS_HH_ -#define SDF_USD_USD_PARSER_TRANSFORMS_HH_ +#ifndef SDF_USD_USD_PARSER_USDTRANSFORMS_HH_ +#define SDF_USD_USD_PARSER_USDTRANSFORMS_HH_ #include #include @@ -70,6 +70,10 @@ namespace sdf /// \brief Rotation /// \return Return a vector with all the rotations + /// If RotationXYZ or RotationZYY is true, this method will return a + /// vector of 3 quaternions, Rotation with the first + /// quaternion being rotation , the second being rotation about + /// , and the third being rotation about public: const std::vector Rotations() const; /// \brief Set translate @@ -81,7 +85,7 @@ namespace sdf public: void Scale(const ignition::math::Vector3d &_scale); /// \brief Add rotation - /// \param[in] _q Add quaterion to the rotation vector + /// \param[in] _q Quaternion to add to the list of rotations public: void AddRotation(const ignition::math::Quaterniond &_q); /// \brief True if there is a rotation ZYX defined or false otherwise @@ -90,47 +94,33 @@ namespace sdf /// \brief True if there is a rotation XYZ defined or false otherwise public: bool RotationXYZ() const; - /// \brief True if there is a rotation (as a quaterion) defined + /// \brief True if there is a rotation (as a quaternion) defined /// or false otherwise public: bool Rotation() const; /// \brief Set if there is any rotation ZYX defined /// RotationZYX is used to determine the order of stored rotations - /// If rotationZYX is true then Rotation should be True too - /// If Rotation is false then rotationZYX could no be true - /// \param[in] _rotationZYX Set if the rotation is ZYX + /// If RotationZYX is true, then Rotation should be True too + /// If Rotation is false, then RotationZYX cannot be true + /// \param[in] _rotationZYX If the rotation is ZYX (true) or not (false) public: void RotationZYX(bool _rotationZYX); /// \brief Set if there is any rotation XYZ defined /// RotationXYZ is used to determine the order of stored rotations - /// If rotationXYZ is true then Rotation should be True too - /// If Rotation is false then rotationXYZ could no be true - /// \param[in] _rotationXYZ Set if the rotation is XYZ + /// If RotationXYZ is true, then Rotation should be True too + /// If Rotation is false, then RotationXYZ cannot be true + /// \param[in] _rotationXYZ If the rotation is XYZ (true) or not (false) public: void RotationXYZ(bool _rotationXYZ); /// \brief Set if there is any rotation defined - /// \param[in] _rotation Set if there is any rotation + /// \param[in] _rotation If there is any rotation defined (true) + /// or not (false) public: void Rotation(bool _rotation); /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) }; - /// \brief This function will parse all the parent transforms of a prim. - /// \param[in] _prim Initial prim to read the transform - /// \param[in] _usdData USDData structure to get info about the prim, for - /// example: metersperunit - /// \param[out] _tfs A vector with all the transforms - /// \param[out] _scale The scale of the prims - /// \param[in] _schemaToStop Name of the prim where the loop will stop - /// reading transforms - void GetAllTransforms( - const pxr::UsdPrim &_prim, - const USDData &_usdData, - std::vector &_tfs, - ignition::math::Vector3d &_scale, - const std::string &_schemaToStop); - /// \brief This function gets the transform from a prim to the specified /// _schemaToStop variable /// \param[in] _prim Initial prim to read the transform @@ -142,7 +132,7 @@ namespace sdf /// reading transforms void IGNITION_SDFORMAT_USD_VISIBLE GetTransform( const pxr::UsdPrim &_prim, - const USDData &_usdData, + USDData &_usdData, ignition::math::Pose3d &_pose, ignition::math::Vector3d &_scale, const std::string &_schemaToStop); diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index af560ad7b..b68488772 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -67,4 +67,9 @@ if (TARGET UNIT_USDPhysics_TEST) usd_parser/USDPhysics.cc) endif() +if (TARGET UNIT_USDTransport_TEST) + target_sources(UNIT_USDTransport_TEST PRIVATE + usd_parser/USDTransforms.cc) +endif() + add_subdirectory(cmd) diff --git a/usd/src/usd_parser/USDData.cc b/usd/src/usd_parser/USDData.cc index e0dcbbc84..535b24c44 100644 --- a/usd/src/usd_parser/USDData.cc +++ b/usd/src/usd_parser/USDData.cc @@ -236,7 +236,7 @@ namespace usd { ///////////////////////////////////////////////// const std::pair> - USDData::FindStage(const std::string &_name) const + USDData::FindStage(const std::string &_name) { for (auto &ref : this->dataPtr->references) { diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index c91a26ff6..245297079 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -16,14 +16,15 @@ */ #include "sdf/usd/usd_parser/USDTransforms.hh" -#include "sdf/usd/usd_parser/USDData.hh" #include #include +#include "sdf/usd/usd_parser/USDData.hh" + namespace sdf { -// Inline bracke to help doxygen filtering. +// Inline bracket to help doxygen filtering. inline namespace SDF_VERSION_NAMESPACE { // namespace usd @@ -132,12 +133,16 @@ bool UDSTransforms::Rotation() const void UDSTransforms::RotationZYX(bool _rotationZYX) { this->dataPtr->isRotationZYX = _rotationZYX; + if (this->dataPtr->isRotationZYX) + this->dataPtr->isRotation = true; } ////////////////////////////////////////////////// void UDSTransforms::RotationXYZ(bool _rotationXYZ) { this->dataPtr->isRotationXYZ = _rotationXYZ; + if (this->dataPtr->isRotationXYZ) + this->dataPtr->isRotation = true; } ////////////////////////////////////////////////// @@ -147,9 +152,17 @@ void UDSTransforms::Rotation(bool _rotation) } ////////////////////////////////////////////////// +/// \brief This function will parse all the parent transforms of a prim. +/// \param[in] _prim Initial prim to read the transform +/// \param[in] _usdData USDData structure to get info about the prim, for +/// example: metersperunit +/// \param[out] _tfs A vector with all the transforms +/// \param[out] _scale The scale of the prims +/// \param[in] _schemaToStop Name of the prim where the loop will stop +/// reading transforms void GetAllTransforms( const pxr::UsdPrim &_prim, - const USDData &_usdData, + USDData &_usdData, std::vector &_tfs, ignition::math::Vector3d &_scale, const std::string &_schemaToStop) @@ -159,8 +172,8 @@ void GetAllTransforms( std::string upAxis = "Y"; // this assumes that there can only be one stage - const auto stageData = _usdData.FindStage(parent.GetPath().GetName()); - if (stageData.second != nullptr) { + auto stageData = _usdData.FindStage(parent.GetPath().GetName()); + if (stageData.second) { metersPerUnit = stageData.second->MetersPerUnit(); upAxis = stageData.second->UpAxis(); } @@ -217,6 +230,7 @@ void GetAllTransforms( parent = parent.GetParent(); } + // Add additional rotation to match with Z up Axis if (upAxis == "Y") { ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( @@ -229,7 +243,7 @@ void GetAllTransforms( ////////////////////////////////////////////////// void GetTransform( const pxr::UsdPrim &_prim, - const USDData &_usdData, + USDData &_usdData, ignition::math::Pose3d &_pose, ignition::math::Vector3d &_scale, const std::string &_schemaToStop) @@ -256,7 +270,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) pxr::VtTokenArray xformOpOrder; transforms.Get(&xformOpOrder); - for (auto & op : xformOpOrder) + for (const auto &op : xformOpOrder) { if (op == kXFormOpScale) { @@ -269,9 +283,9 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) { pxr::GfVec3d scaleTmp(1, 1, 1); attribute.Get(&scaleTmp); - scale[0] = scaleTmp[0]; - scale[1] = scaleTmp[1]; - scale[2] = scaleTmp[2]; + scale[0] = static_cast(scaleTmp[0]); + scale[1] = static_cast(scaleTmp[1]); + scale[2] = static_cast(scaleTmp[2]); } t.Scale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); } @@ -297,9 +311,9 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) { pxr::GfVec3d rotationEulerTmp(0, 0, 0); attribute.Get(&rotationEulerTmp); - rotationEuler[0] = rotationEulerTmp[0]; - rotationEuler[1] = rotationEulerTmp[1]; - rotationEuler[2] = rotationEulerTmp[2]; + rotationEuler[0] = static_cast(rotationEulerTmp[0]); + rotationEuler[1] = static_cast(rotationEulerTmp[1]); + rotationEuler[2] = static_cast(rotationEulerTmp[2]); } ignition::math::Quaterniond qX, qY, qZ; ignition::math::Angle angleX(IGN_DTOR(rotationEuler[0])); @@ -325,9 +339,9 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) { pxr::GfVec3d translateTmp(0, 0, 0); attribute.Get(&translateTmp); - translate[0] = translateTmp[0]; - translate[1] = translateTmp[1]; - translate[2] = translateTmp[2]; + translate[0] = static_cast(translateTmp[0]); + translate[1] = static_cast(translateTmp[1]); + translate[2] = static_cast(translateTmp[2]); } t.Translate(ignition::math::Vector3d( translate[0], @@ -341,7 +355,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) { attribute.Get(&rotationQuad); } - else if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatfString) + else if (attribute.GetTypeName().GetCPPTypeName() == kGfQuatdString) { pxr::GfQuatd rotationQuadTmp; attribute.Get(&rotationQuadTmp); @@ -362,7 +376,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) if (op == kXFormOpTransform) { - // FIXME: Shear is lost (does sdformat support it?). + // TODO(koonpeng) Shear is lost (does sdformat support it?). pxr::GfMatrix4d transform; _prim.GetAttribute(pxr::TfToken(kXFormOpTransform)).Get(&transform); const auto rot = transform.RemoveScaleShear(); diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index 3923d7955..c410c5213 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -51,12 +51,17 @@ void checkTransforms( EXPECT_EQ(_translation, usdTransforms.Translate()); EXPECT_EQ(_scale, usdTransforms.Scale()); EXPECT_EQ(_rotation.size(), usdTransforms.Rotations().size()); + ASSERT_EQ(_rotation.size(), usdTransforms.Rotations().size()); for (unsigned int i = 0; i < _rotation.size(); ++i) { EXPECT_EQ(_rotation[i], usdTransforms.Rotations()[i]); } EXPECT_EQ(!_rotation.empty(), usdTransforms.Rotation()); + if (usdTransforms.RotationXYZ() || usdTransforms.RotationZYX()) + { + EXPECT_TRUE(usdTransforms.Rotation()); + } } ///////////////////////////////////////////////// @@ -174,7 +179,7 @@ TEST(Utils, GetAllTransform) auto stage = pxr::UsdStage::Open(filename); ASSERT_TRUE(stage); - sdf::usd::USDData usdData(sdf::testing::TestFile("usd", "upAxisZ.usda")); + sdf::usd::USDData usdData(filename); usdData.Init(); pxr::UsdPrim prim = stage->GetPrimAtPath( From 3ce73a12644125da48c81b258c007ff07f19a354 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 24 Mar 2022 19:09:57 +0100 Subject: [PATCH 24/33] Add feedback Signed-off-by: ahcorde --- test/usd/upAxisZ.usda | 4 +- .../sdf/usd/usd_parser/USDTransforms.hh | 15 ++--- usd/src/usd_parser/USDTransforms.cc | 62 ++++++++----------- usd/src/usd_parser/USDTransforms_TEST.cc | 12 ++-- 4 files changed, 40 insertions(+), 53 deletions(-) diff --git a/test/usd/upAxisZ.usda b/test/usd/upAxisZ.usda index ea76aa750..a40ab7129 100644 --- a/test/usd/upAxisZ.usda +++ b/test/usd/upAxisZ.usda @@ -191,7 +191,7 @@ def "shapes" def Xform "ellipsoid" { - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateXYZ = (15, 80, -55) double3 xformOp:translate = (0, 3, 0.5) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] @@ -230,7 +230,7 @@ def "shapes" { float inputs:intensity = 1000 float intensity = 100 - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateXYZ = (0, -35, 0) double3 xformOp:translate = (0, 0, 10) uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] } diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 04664e70c..7526df2d5 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -62,7 +62,7 @@ namespace sdf /// \brief Translate /// \return A 3D vector with the translation - public: const ignition::math::Vector3d Translate() const; + public: const ignition::math::Vector3d Translation() const; /// \brief Scale /// \return A 3D vector with the scale @@ -78,11 +78,11 @@ namespace sdf /// \brief Set translate /// \param[in] _translate Translate to set - public: void Translate(const ignition::math::Vector3d &_translate); + public: void SetTranslation(const ignition::math::Vector3d &_translate); /// \brief Set scale /// \param[in] _scale Scale to set - public: void Scale(const ignition::math::Vector3d &_scale); + public: void SetScale(const ignition::math::Vector3d &_scale); /// \brief Add rotation /// \param[in] _q Quaternion to add to the list of rotations @@ -103,19 +103,14 @@ namespace sdf /// If RotationZYX is true, then Rotation should be True too /// If Rotation is false, then RotationZYX cannot be true /// \param[in] _rotationZYX If the rotation is ZYX (true) or not (false) - public: void RotationZYX(bool _rotationZYX); + public: void SetRotationZYX(bool _rotationZYX); /// \brief Set if there is any rotation XYZ defined /// RotationXYZ is used to determine the order of stored rotations /// If RotationXYZ is true, then Rotation should be True too /// If Rotation is false, then RotationXYZ cannot be true /// \param[in] _rotationXYZ If the rotation is XYZ (true) or not (false) - public: void RotationXYZ(bool _rotationXYZ); - - /// \brief Set if there is any rotation defined - /// \param[in] _rotation If there is any rotation defined (true) - /// or not (false) - public: void Rotation(bool _rotation); + public: void SetRotationXYZ(bool _rotationXYZ); /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index 245297079..1ed665d6e 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -59,10 +59,6 @@ class UDSTransforms::Implementation /// \brief True if there is a rotation XYZ defined or false otherwise public: bool isRotationXYZ = false; - - /// \brief True if there is a rotation (as a quaterion) defined - /// or false otherwise - public: bool isRotation = false; }; ///////////////////////////////////////////////// @@ -72,7 +68,7 @@ UDSTransforms::UDSTransforms() } ////////////////////////////////////////////////// -const ignition::math::Vector3d UDSTransforms::Translate() const +const ignition::math::Vector3d UDSTransforms::Translation() const { return this->dataPtr->translate; } @@ -91,14 +87,14 @@ const std::vector } ////////////////////////////////////////////////// -void UDSTransforms::Translate( +void UDSTransforms::SetTranslation( const ignition::math::Vector3d &_translate) { this->dataPtr->translate = _translate; } ////////////////////////////////////////////////// -void UDSTransforms::Scale( +void UDSTransforms::SetScale( const ignition::math::Vector3d &_scale) { this->dataPtr->scale = _scale; @@ -126,29 +122,19 @@ bool UDSTransforms::RotationXYZ() const ////////////////////////////////////////////////// bool UDSTransforms::Rotation() const { - return this->dataPtr->isRotation; + return !this->dataPtr->q.empty(); } ////////////////////////////////////////////////// -void UDSTransforms::RotationZYX(bool _rotationZYX) +void UDSTransforms::SetRotationZYX(bool _rotationZYX) { this->dataPtr->isRotationZYX = _rotationZYX; - if (this->dataPtr->isRotationZYX) - this->dataPtr->isRotation = true; } ////////////////////////////////////////////////// -void UDSTransforms::RotationXYZ(bool _rotationXYZ) +void UDSTransforms::SetRotationXYZ(bool _rotationXYZ) { this->dataPtr->isRotationXYZ = _rotationXYZ; - if (this->dataPtr->isRotationXYZ) - this->dataPtr->isRotation = true; -} - -////////////////////////////////////////////////// -void UDSTransforms::Rotation(bool _rotation) -{ - this->dataPtr->isRotation = _rotation; } ////////////////////////////////////////////////// @@ -190,7 +176,7 @@ void GetAllTransforms( ignition::math::Pose3d pose; _scale *= t.Scale(); - pose.Pos() = t.Translate() * metersPerUnit; + pose.Pos() = t.Translation() * metersPerUnit; // scaling is lost when we convert to pose, so we pre-scale the // translation to make them match the scaled values. if (!_tfs.empty()) { @@ -219,20 +205,29 @@ void GetAllTransforms( ignition::math::Vector3d(0, 0, 0), t.Rotations()[0]); ignition::math::Pose3d poseT = ignition::math::Pose3d( - t.Translate() * metersPerUnit, + t.Translation() * metersPerUnit, ignition::math::Quaterniond(1, 0, 0, 0)); - _tfs.push_back(poseZ); - _tfs.push_back(poseY); - _tfs.push_back(poseX); + if (t.RotationZYX()) + { + _tfs.push_back(poseZ); + _tfs.push_back(poseY); + _tfs.push_back(poseX); + } + else if (t.RotationXYZ()) + { + _tfs.push_back(poseX); + _tfs.push_back(poseY); + _tfs.push_back(poseZ); + } _tfs.push_back(poseT); } parent = parent.GetParent(); } - // Add additional rotation to match with Z up Axis if (upAxis == "Y") { + // Add additional rotation to match with Z up Axis ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( ignition::math::Vector3d(0, 0, 0), ignition::math::Quaterniond(IGN_PI_2, 0, 0)); @@ -287,7 +282,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) scale[1] = static_cast(scaleTmp[1]); scale[2] = static_cast(scaleTmp[2]); } - t.Scale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); + t.SetScale(ignition::math::Vector3d(scale[0], scale[1], scale[2])); } else if (op == kXFormOpRotateZYX || op == kXFormOpRotateXYZ) { @@ -296,12 +291,12 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) if (op == kXFormOpRotateZYX) { attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateZYX)); - t.RotationZYX(true); + t.SetRotationZYX(true); } else { attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateXYZ)); - t.RotationXYZ(true); + t.SetRotationXYZ(true); } if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) { @@ -326,7 +321,6 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) t.AddRotation(qX); t.AddRotation(qY); t.AddRotation(qZ); - t.Rotation(true); } else if (op == kXFormOpTranslate) { @@ -343,7 +337,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) translate[1] = static_cast(translateTmp[1]); translate[2] = static_cast(translateTmp[2]); } - t.Translate(ignition::math::Vector3d( + t.SetTranslation(ignition::math::Vector3d( translate[0], translate[1], translate[2])); @@ -371,7 +365,6 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) rotationQuad.GetImaginary()[1], rotationQuad.GetImaginary()[2]); t.AddRotation(q); - t.Rotation(true); } if (op == kXFormOpTransform) @@ -382,13 +375,13 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) const auto rot = transform.RemoveScaleShear(); const auto scaleShear = transform * rot.GetInverse(); - t.Scale(ignition::math::Vector3d( + t.SetScale(ignition::math::Vector3d( scaleShear[0][0], scaleShear[1][1], scaleShear[2][2])); const auto rotQuat = rot.ExtractRotationQuat(); - t.Translate(ignition::math::Vector3d( + t.SetTranslation(ignition::math::Vector3d( transform[3][0], transform[3][1], transform[3][2])); @@ -399,7 +392,6 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) rotQuat.GetImaginary()[2] ); t.AddRotation(q); - t.Rotation(true); } } return t; diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index c410c5213..098337a90 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -48,7 +48,7 @@ void checkTransforms( sdf::usd::UDSTransforms usdTransforms = sdf::usd::ParseUSDTransform(prim); - EXPECT_EQ(_translation, usdTransforms.Translate()); + EXPECT_EQ(_translation, usdTransforms.Translation()); EXPECT_EQ(_scale, usdTransforms.Scale()); EXPECT_EQ(_rotation.size(), usdTransforms.Rotations().size()); ASSERT_EQ(_rotation.size(), usdTransforms.Rotations().size()); @@ -144,9 +144,9 @@ TEST(Utils, GetTransform) stage, ignition::math::Vector3d(0, 3.0, 0.5), { - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0) + ignition::math::Quaterniond(IGN_DTOR(15), 0, 0), + ignition::math::Quaterniond(0, IGN_DTOR(80), 0), + ignition::math::Quaterniond(0, 0, IGN_DTOR(-55)) }, ignition::math::Vector3d(1, 1, 1) ); @@ -165,7 +165,7 @@ TEST(Utils, GetTransform) ignition::math::Vector3d(0, 0, 10), { ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Quaterniond(0, IGN_DTOR(-35), 0), ignition::math::Quaterniond(1, 0, 0, 0) }, ignition::math::Vector3d(1, 1, 1) @@ -195,6 +195,6 @@ TEST(Utils, GetAllTransform) EXPECT_EQ( ignition::math::Pose3d( ignition::math::Vector3d(0, 0.03, 0.005), - ignition::math::Quaterniond(1, 0, 0, 0)), + ignition::math::Quaterniond(IGN_DTOR(15), IGN_DTOR(80), IGN_DTOR(-55))), pose); } From 479330f9b728c4d9fde3f373da39564625c5f7ea Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 28 Mar 2022 10:36:51 +0200 Subject: [PATCH 25/33] Convert FindStage to const Signed-off-by: ahcorde --- usd/include/sdf/usd/usd_parser/USDData.hh | 2 +- usd/include/sdf/usd/usd_parser/USDTransforms.hh | 2 +- usd/src/usd_parser/USDData.cc | 2 +- usd/src/usd_parser/USDTransforms.cc | 6 +++--- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/usd/include/sdf/usd/usd_parser/USDData.hh b/usd/include/sdf/usd/usd_parser/USDData.hh index d76e861dd..9936858cb 100644 --- a/usd/include/sdf/usd/usd_parser/USDData.hh +++ b/usd/include/sdf/usd/usd_parser/USDData.hh @@ -82,7 +82,7 @@ namespace sdf /// \param[in] _name Name of the path to find /// \return A pair with the name of the stage and the data public: const std::pair> - FindStage(const std::string &_name); + FindStage(const std::string &_name) const; public: friend std::ostream& operator<<( std::ostream& os, const USDData& data) diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index 7526df2d5..b5c0306b3 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -127,7 +127,7 @@ namespace sdf /// reading transforms void IGNITION_SDFORMAT_USD_VISIBLE GetTransform( const pxr::UsdPrim &_prim, - USDData &_usdData, + const USDData &_usdData, ignition::math::Pose3d &_pose, ignition::math::Vector3d &_scale, const std::string &_schemaToStop); diff --git a/usd/src/usd_parser/USDData.cc b/usd/src/usd_parser/USDData.cc index 535b24c44..e0dcbbc84 100644 --- a/usd/src/usd_parser/USDData.cc +++ b/usd/src/usd_parser/USDData.cc @@ -236,7 +236,7 @@ namespace usd { ///////////////////////////////////////////////// const std::pair> - USDData::FindStage(const std::string &_name) + USDData::FindStage(const std::string &_name) const { for (auto &ref : this->dataPtr->references) { diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index 1ed665d6e..cc5bcf071 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -148,7 +148,7 @@ void UDSTransforms::SetRotationXYZ(bool _rotationXYZ) /// reading transforms void GetAllTransforms( const pxr::UsdPrim &_prim, - USDData &_usdData, + const USDData &_usdData, std::vector &_tfs, ignition::math::Vector3d &_scale, const std::string &_schemaToStop) @@ -158,7 +158,7 @@ void GetAllTransforms( std::string upAxis = "Y"; // this assumes that there can only be one stage - auto stageData = _usdData.FindStage(parent.GetPath().GetName()); + const auto stageData = _usdData.FindStage(parent.GetPath().GetName()); if (stageData.second) { metersPerUnit = stageData.second->MetersPerUnit(); upAxis = stageData.second->UpAxis(); @@ -238,7 +238,7 @@ void GetAllTransforms( ////////////////////////////////////////////////// void GetTransform( const pxr::UsdPrim &_prim, - USDData &_usdData, + const USDData &_usdData, ignition::math::Pose3d &_pose, ignition::math::Vector3d &_scale, const std::string &_schemaToStop) From 0198bfa913496bcf184f48375294a3025df33bfb Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 28 Mar 2022 12:44:29 +0200 Subject: [PATCH 26/33] Added rotateZYX test Signed-off-by: ahcorde --- test/usd/upAxisZ.usda | 8 ++++---- usd/src/usd_parser/USDTransforms_TEST.cc | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/test/usd/upAxisZ.usda b/test/usd/upAxisZ.usda index a40ab7129..72f3527f5 100644 --- a/test/usd/upAxisZ.usda +++ b/test/usd/upAxisZ.usda @@ -120,9 +120,9 @@ def "shapes" def Xform "sphere" { - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateZYX = (-69, 31, -62) double3 xformOp:translate = (0, 1.5, 0.5) - uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] def Xform "sphere_link" ( prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] @@ -155,9 +155,9 @@ def "shapes" def Xform "capsule" { - float3 xformOp:rotateXYZ = (0, 0, 0) + float3 xformOp:rotateZYX = (15, 80, -55) double3 xformOp:translate = (0, -3, 0.5) - uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] def Xform "capsule_link" ( prepend apiSchemas = ["PhysicsRigidBodyAPI", "PhysicsMassAPI"] diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index 098337a90..a5c6926ed 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -108,9 +108,9 @@ TEST(Utils, GetTransform) stage, ignition::math::Vector3d(0, 1.5, 0.5), { - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0) + ignition::math::Quaterniond(IGN_DTOR(-69), 0, 0), + ignition::math::Quaterniond(0, IGN_DTOR(31), 0), + ignition::math::Quaterniond(0, 0, IGN_DTOR(-62)) }, ignition::math::Vector3d(1, 1, 1) ); @@ -120,9 +120,9 @@ TEST(Utils, GetTransform) stage, ignition::math::Vector3d(0, -3.0, 0.5), { - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0) + ignition::math::Quaterniond(IGN_DTOR(15), 0, 0), + ignition::math::Quaterniond(0, IGN_DTOR(80), 0), + ignition::math::Quaterniond(0, 0, IGN_DTOR(-55)) }, ignition::math::Vector3d(1, 1, 1) ); From b27019219882eb4425481e1b94f1587c26988f53 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Mon, 28 Mar 2022 18:55:48 -0400 Subject: [PATCH 27/33] remove unneeded cmake code Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- usd/src/CMakeLists.txt | 5 ----- 1 file changed, 5 deletions(-) diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index 0c88be656..e90c283eb 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -67,11 +67,6 @@ if (TARGET UNIT_USDPhysics_TEST) usd_parser/USDPhysics.cc) endif() -if (TARGET UNIT_USDTransport_TEST) - target_sources(UNIT_USDTransport_TEST PRIVATE - usd_parser/USDTransforms.cc) -endif() - if (TARGET UNIT_Conversions_TEST) target_sources(UNIT_Conversions_TEST PRIVATE Conversions.cc) From e2acde3f93cc5be5bb136851cc8f813e6753d430 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Mon, 28 Mar 2022 18:56:34 -0400 Subject: [PATCH 28/33] make sure only XYZ or ZYX rotation is enabled, but not both Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- usd/src/usd_parser/USDTransforms.cc | 10 ++++++++-- usd/src/usd_parser/USDTransforms_TEST.cc | 5 ++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index cc5bcf071..1fad0c368 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -51,7 +51,7 @@ class UDSTransforms::Implementation /// \brief Rotation of the schema public: std::vector q; - /// \brief Translatio of the schema + /// \brief Translation of the schema public: ignition::math::Vector3d translate{0, 0, 0}; /// \brief True if there is a rotation ZYX defined or false otherwise @@ -129,12 +129,16 @@ bool UDSTransforms::Rotation() const void UDSTransforms::SetRotationZYX(bool _rotationZYX) { this->dataPtr->isRotationZYX = _rotationZYX; + if (_rotationZYX) + this->dataPtr->isRotationXYZ = false; } ////////////////////////////////////////////////// void UDSTransforms::SetRotationXYZ(bool _rotationXYZ) { this->dataPtr->isRotationXYZ = _rotationXYZ; + if (_rotationXYZ) + this->dataPtr->isRotationZYX = false; } ////////////////////////////////////////////////// @@ -227,7 +231,9 @@ void GetAllTransforms( if (upAxis == "Y") { - // Add additional rotation to match with Z up Axis + // Add additional rotation to match with Z up Axis. + // TODO(anyone) handle upAxis == "X". This is a case that is rarely + // used by other renderers ignition::math::Pose3d poseUpAxis = ignition::math::Pose3d( ignition::math::Vector3d(0, 0, 0), ignition::math::Quaterniond(IGN_PI_2, 0, 0)); diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index a5c6926ed..8737a267f 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -50,18 +50,21 @@ void checkTransforms( EXPECT_EQ(_translation, usdTransforms.Translation()); EXPECT_EQ(_scale, usdTransforms.Scale()); - EXPECT_EQ(_rotation.size(), usdTransforms.Rotations().size()); ASSERT_EQ(_rotation.size(), usdTransforms.Rotations().size()); for (unsigned int i = 0; i < _rotation.size(); ++i) { EXPECT_EQ(_rotation[i], usdTransforms.Rotations()[i]); } + bool checkedXYZorZXY = false; EXPECT_EQ(!_rotation.empty(), usdTransforms.Rotation()); if (usdTransforms.RotationXYZ() || usdTransforms.RotationZYX()) { EXPECT_TRUE(usdTransforms.Rotation()); + EXPECT_NE(usdTransforms.RotationXYZ(), usdTransforms.RotationZYX()); + checkedXYZorZXY = true; } + EXPECT_EQ(!_rotation.empty(), checkedXYZorZXY); } ///////////////////////////////////////////////// From efbbc636b2106a014435e6f172c5ed681c306473 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Wed, 30 Mar 2022 10:35:07 -0600 Subject: [PATCH 29/33] be more explicit about testing XYZ vs ZYX rotation Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- usd/src/usd_parser/USDTransforms_TEST.cc | 54 ++++++++++++++++-------- 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index 8737a267f..f994ad1b8 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -40,7 +40,9 @@ void checkTransforms( pxr::UsdStageRefPtr &_stage, const ignition::math::Vector3d &_translation, const std::vector &_rotation, - const ignition::math::Vector3d &_scale) + const ignition::math::Vector3d &_scale, + bool isXYZRotation, + bool isZYXRotation) { pxr::UsdPrim prim = _stage->GetPrimAtPath(pxr::SdfPath(_primName)); ASSERT_TRUE(prim); @@ -56,19 +58,19 @@ void checkTransforms( EXPECT_EQ(_rotation[i], usdTransforms.Rotations()[i]); } - bool checkedXYZorZXY = false; EXPECT_EQ(!_rotation.empty(), usdTransforms.Rotation()); - if (usdTransforms.RotationXYZ() || usdTransforms.RotationZYX()) + EXPECT_EQ(isXYZRotation, usdTransforms.RotationXYZ()); + EXPECT_EQ(isZYXRotation, usdTransforms.RotationZYX()); + const bool hasRotation = isXYZRotation || isZYXRotation; + if (hasRotation) { - EXPECT_TRUE(usdTransforms.Rotation()); EXPECT_NE(usdTransforms.RotationXYZ(), usdTransforms.RotationZYX()); - checkedXYZorZXY = true; } - EXPECT_EQ(!_rotation.empty(), checkedXYZorZXY); + EXPECT_EQ(hasRotation, usdTransforms.Rotation()); } ///////////////////////////////////////////////// -TEST(Utils, GetTransform) +TEST(USDTransformsTest, ParseUSDTransform) { std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); auto stage = pxr::UsdStage::Open(filename); @@ -83,7 +85,9 @@ TEST(Utils, GetTransform) ignition::math::Quaterniond(1, 0, 0, 0), ignition::math::Quaterniond(1, 0, 0, 0) }, - ignition::math::Vector3d(1, 1, 1) + ignition::math::Vector3d(1, 1, 1), + true, + false ); checkTransforms( @@ -91,7 +95,9 @@ TEST(Utils, GetTransform) stage, ignition::math::Vector3d(0, 0, 0), {}, - ignition::math::Vector3d(100, 100, 0.25) + ignition::math::Vector3d(100, 100, 0.25), + false, + false ); checkTransforms( @@ -103,7 +109,9 @@ TEST(Utils, GetTransform) ignition::math::Quaterniond(1, 0, 0, 0), ignition::math::Quaterniond(1, 0, 0, 0) }, - ignition::math::Vector3d(1, 1, 1) + ignition::math::Vector3d(1, 1, 1), + true, + false ); checkTransforms( @@ -115,7 +123,9 @@ TEST(Utils, GetTransform) ignition::math::Quaterniond(0, IGN_DTOR(31), 0), ignition::math::Quaterniond(0, 0, IGN_DTOR(-62)) }, - ignition::math::Vector3d(1, 1, 1) + ignition::math::Vector3d(1, 1, 1), + false, + true ); checkTransforms( @@ -127,7 +137,9 @@ TEST(Utils, GetTransform) ignition::math::Quaterniond(0, IGN_DTOR(80), 0), ignition::math::Quaterniond(0, 0, IGN_DTOR(-55)) }, - ignition::math::Vector3d(1, 1, 1) + ignition::math::Vector3d(1, 1, 1), + false, + true ); checkTransforms( @@ -139,7 +151,9 @@ TEST(Utils, GetTransform) ignition::math::Quaterniond(1, 0, 0, 0), ignition::math::Quaterniond(0, 0, M_PI_2) }, - ignition::math::Vector3d(1, 1, 1) + ignition::math::Vector3d(1, 1, 1), + false, + true ); checkTransforms( @@ -151,7 +165,9 @@ TEST(Utils, GetTransform) ignition::math::Quaterniond(0, IGN_DTOR(80), 0), ignition::math::Quaterniond(0, 0, IGN_DTOR(-55)) }, - ignition::math::Vector3d(1, 1, 1) + ignition::math::Vector3d(1, 1, 1), + true, + false ); checkTransforms( @@ -159,7 +175,9 @@ TEST(Utils, GetTransform) stage, ignition::math::Vector3d(0, 0, 0), {}, - ignition::math::Vector3d(0.4, 0.6, 1) + ignition::math::Vector3d(0.4, 0.6, 1), + false, + false ); checkTransforms( @@ -171,12 +189,14 @@ TEST(Utils, GetTransform) ignition::math::Quaterniond(0, IGN_DTOR(-35), 0), ignition::math::Quaterniond(1, 0, 0, 0) }, - ignition::math::Vector3d(1, 1, 1) + ignition::math::Vector3d(1, 1, 1), + true, + false ); } ///////////////////////////////////////////////// -TEST(Utils, GetAllTransform) +TEST(USDTransformsTest, GetAllTransform) { std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); auto stage = pxr::UsdStage::Open(filename); From da475d4bd6260487a77657c12a457d6a3b8c3c93 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Wed, 30 Mar 2022 14:08:26 -0600 Subject: [PATCH 30/33] test nested position transforms Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- test/usd/nested_transforms.usda | 22 +++++++++ usd/src/usd_parser/USDTransforms_TEST.cc | 61 +++++++++++++++++------- 2 files changed, 66 insertions(+), 17 deletions(-) create mode 100644 test/usd/nested_transforms.usda diff --git a/test/usd/nested_transforms.usda b/test/usd/nested_transforms.usda new file mode 100644 index 000000000..b86240cb0 --- /dev/null +++ b/test/usd/nested_transforms.usda @@ -0,0 +1,22 @@ +#usda 1.0 +( + metersPerUnit = 0.01 + upAxis = "Z" +) + +def "transforms" +{ + def Xform "nested_transforms_XYZ" + { + float3 xformOp:rotateXYZ = (0, 0, 90) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "child_transform" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + } + } +} diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index f994ad1b8..d422037a6 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -198,26 +198,53 @@ TEST(USDTransformsTest, ParseUSDTransform) ///////////////////////////////////////////////// TEST(USDTransformsTest, GetAllTransform) { - std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); - auto stage = pxr::UsdStage::Open(filename); - ASSERT_TRUE(stage); + { + std::string filename = sdf::testing::TestFile("usd", "upAxisZ.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); - sdf::usd::USDData usdData(filename); - usdData.Init(); + sdf::usd::USDData usdData(filename); + usdData.Init(); - pxr::UsdPrim prim = stage->GetPrimAtPath( - pxr::SdfPath("/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")); - ASSERT_TRUE(prim); + pxr::UsdPrim prim = stage->GetPrimAtPath( + pxr::SdfPath("/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")); + ASSERT_TRUE(prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/shapes"); - ignition::math::Pose3d pose; - ignition::math::Vector3d scale{1, 1, 1}; + EXPECT_EQ(ignition::math::Vector3d(0.4, 0.6, 1), scale); + EXPECT_EQ( + ignition::math::Pose3d( + ignition::math::Vector3d(0, 0.03, 0.005), + ignition::math::Quaterniond(IGN_DTOR(15), IGN_DTOR(80), IGN_DTOR(-55))), + pose); + } + + { + std::string filename = sdf::testing::TestFile("usd", "nested_transforms.usda"); + auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); - sdf::usd::GetTransform(prim, usdData, pose, scale, "/shapes"); + const pxr::UsdPrim prim = stage->GetPrimAtPath( + pxr::SdfPath("/transforms/nested_transforms_XYZ/child_transform")); + ASSERT_TRUE(prim); - EXPECT_EQ(ignition::math::Vector3d(0.4, 0.6, 1), scale); - EXPECT_EQ( - ignition::math::Pose3d( - ignition::math::Vector3d(0, 0.03, 0.005), - ignition::math::Quaterniond(IGN_DTOR(15), IGN_DTOR(80), IGN_DTOR(-55))), - pose); + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/transforms"); + + EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), scale); + EXPECT_EQ( + ignition::math::Pose3d( + ignition::math::Vector3d(.01, .01, 0), + ignition::math::Quaterniond(0, 0, IGN_DTOR(90))), + pose); + } } From 68b26c903b9112f1fc3a049e8c500b607884f3ca Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 30 Mar 2022 23:24:05 +0200 Subject: [PATCH 31/33] make linters happy Signed-off-by: ahcorde --- usd/src/usd_parser/USDTransforms_TEST.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index d422037a6..179cd29aa 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -207,7 +207,8 @@ TEST(USDTransformsTest, GetAllTransform) usdData.Init(); pxr::UsdPrim prim = stage->GetPrimAtPath( - pxr::SdfPath("/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")); + pxr::SdfPath( + "/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry")); ASSERT_TRUE(prim); ignition::math::Pose3d pose; @@ -224,7 +225,8 @@ TEST(USDTransformsTest, GetAllTransform) } { - std::string filename = sdf::testing::TestFile("usd", "nested_transforms.usda"); + std::string filename = + sdf::testing::TestFile("usd", "nested_transforms.usda"); auto stage = pxr::UsdStage::Open(filename); ASSERT_TRUE(stage); From cf28342d5f8876d110ceb56f75de6f9c9a7fd977 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Thu, 31 Mar 2022 04:56:49 -0600 Subject: [PATCH 32/33] USD -> SDF transform fixes (#924) Signed-off-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Co-authored-by: ahcorde --- test/usd/nested_transforms.usda | 14 ++ .../sdf/usd/usd_parser/USDTransforms.hh | 43 +---- usd/src/usd_parser/USDTransforms.cc | 102 ++--------- usd/src/usd_parser/USDTransforms_TEST.cc | 161 +++++++----------- 4 files changed, 103 insertions(+), 217 deletions(-) diff --git a/test/usd/nested_transforms.usda b/test/usd/nested_transforms.usda index b86240cb0..32e46edf4 100644 --- a/test/usd/nested_transforms.usda +++ b/test/usd/nested_transforms.usda @@ -19,4 +19,18 @@ def "transforms" uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] } } + + def Xform "nested_transforms_ZYX" + { + float3 xformOp:rotateZYX = (90, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + + def Xform "child_transform" + { + float3 xformOp:rotateZYX = (0, 0, 0) + double3 xformOp:translate = (1, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateZYX"] + } + } } diff --git a/usd/include/sdf/usd/usd_parser/USDTransforms.hh b/usd/include/sdf/usd/usd_parser/USDTransforms.hh index b5c0306b3..4b0d75da5 100644 --- a/usd/include/sdf/usd/usd_parser/USDTransforms.hh +++ b/usd/include/sdf/usd/usd_parser/USDTransforms.hh @@ -53,8 +53,6 @@ namespace sdf /// This might contain scale, translate or rotation operations /// The booleans are used to check if there is a transform defined /// in the schema - /// Rotation is splitted in a vector because this might be defined - /// as a rotation of 3 angles (ZYX, XYZ, etc). class IGNITION_SDFORMAT_USD_VISIBLE UDSTransforms { /// \brief Default constructor @@ -68,13 +66,10 @@ namespace sdf /// \return A 3D vector with the scale public: const ignition::math::Vector3d Scale() const; - /// \brief Rotation - /// \return Return a vector with all the rotations - /// If RotationXYZ or RotationZYY is true, this method will return a - /// vector of 3 quaternions, Rotation with the first - /// quaternion being rotation , the second being rotation about - /// , and the third being rotation about - public: const std::vector Rotations() const; + /// \brief Get the Rotation + /// \return Return The rotation, if one exists. If no rotation exists, + /// std::nullopt is returned + public: const std::optional Rotation() const; /// \brief Set translate /// \param[in] _translate Translate to set @@ -84,33 +79,9 @@ namespace sdf /// \param[in] _scale Scale to set public: void SetScale(const ignition::math::Vector3d &_scale); - /// \brief Add rotation - /// \param[in] _q Quaternion to add to the list of rotations - public: void AddRotation(const ignition::math::Quaterniond &_q); - - /// \brief True if there is a rotation ZYX defined or false otherwise - public: bool RotationZYX() const; - - /// \brief True if there is a rotation XYZ defined or false otherwise - public: bool RotationXYZ() const; - - /// \brief True if there is a rotation (as a quaternion) defined - /// or false otherwise - public: bool Rotation() const; - - /// \brief Set if there is any rotation ZYX defined - /// RotationZYX is used to determine the order of stored rotations - /// If RotationZYX is true, then Rotation should be True too - /// If Rotation is false, then RotationZYX cannot be true - /// \param[in] _rotationZYX If the rotation is ZYX (true) or not (false) - public: void SetRotationZYX(bool _rotationZYX); - - /// \brief Set if there is any rotation XYZ defined - /// RotationXYZ is used to determine the order of stored rotations - /// If RotationXYZ is true, then Rotation should be True too - /// If Rotation is false, then RotationXYZ cannot be true - /// \param[in] _rotationXYZ If the rotation is XYZ (true) or not (false) - public: void SetRotationXYZ(bool _rotationXYZ); + /// \brief Set rotation + /// \param[in] _q Quaternion that defines the rotation + public: void SetRotation(const ignition::math::Quaterniond &_q); /// \brief Private data pointer. IGN_UTILS_IMPL_PTR(dataPtr) diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index 1fad0c368..216394c86 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -17,6 +17,9 @@ #include "sdf/usd/usd_parser/USDTransforms.hh" +#include +#include + #include #include @@ -49,16 +52,10 @@ class UDSTransforms::Implementation public: ignition::math::Vector3d scale{1, 1, 1}; /// \brief Rotation of the schema - public: std::vector q; + public: std::optional q = std::nullopt; /// \brief Translation of the schema public: ignition::math::Vector3d translate{0, 0, 0}; - - /// \brief True if there is a rotation ZYX defined or false otherwise - public: bool isRotationZYX = false; - - /// \brief True if there is a rotation XYZ defined or false otherwise - public: bool isRotationXYZ = false; }; ///////////////////////////////////////////////// @@ -80,8 +77,7 @@ const ignition::math::Vector3d UDSTransforms::Scale() const } ////////////////////////////////////////////////// -const std::vector - UDSTransforms::Rotations() const +const std::optional UDSTransforms::Rotation() const { return this->dataPtr->q; } @@ -101,44 +97,10 @@ void UDSTransforms::SetScale( } ////////////////////////////////////////////////// -void UDSTransforms::AddRotation( +void UDSTransforms::SetRotation( const ignition::math::Quaterniond &_q) { - this->dataPtr->q.push_back(_q); -} - -////////////////////////////////////////////////// -bool UDSTransforms::RotationZYX() const -{ - return this->dataPtr->isRotationZYX; -} - -////////////////////////////////////////////////// -bool UDSTransforms::RotationXYZ() const -{ - return this->dataPtr->isRotationXYZ; -} - -////////////////////////////////////////////////// -bool UDSTransforms::Rotation() const -{ - return !this->dataPtr->q.empty(); -} - -////////////////////////////////////////////////// -void UDSTransforms::SetRotationZYX(bool _rotationZYX) -{ - this->dataPtr->isRotationZYX = _rotationZYX; - if (_rotationZYX) - this->dataPtr->isRotationXYZ = false; -} - -////////////////////////////////////////////////// -void UDSTransforms::SetRotationXYZ(bool _rotationXYZ) -{ - this->dataPtr->isRotationXYZ = _rotationXYZ; - if (_rotationXYZ) - this->dataPtr->isRotationZYX = false; + this->dataPtr->q = _q; } ////////////////////////////////////////////////// @@ -191,41 +153,11 @@ void GetAllTransforms( child.Pos().Z() * t.Scale()[2]); } - if (!t.RotationZYX() && !t.RotationXYZ()) + if (t.Rotation()) { - if (t.Rotation()) - { - pose.Rot() = t.Rotations()[0]; - } - _tfs.push_back(pose); - } - else - { - ignition::math::Pose3d poseZ = ignition::math::Pose3d( - ignition::math::Vector3d(0, 0, 0), t.Rotations()[2]); - ignition::math::Pose3d poseY = ignition::math::Pose3d( - ignition::math::Vector3d(0, 0, 0), t.Rotations()[1]); - ignition::math::Pose3d poseX = ignition::math::Pose3d( - ignition::math::Vector3d(0, 0, 0), t.Rotations()[0]); - - ignition::math::Pose3d poseT = ignition::math::Pose3d( - t.Translation() * metersPerUnit, - ignition::math::Quaterniond(1, 0, 0, 0)); - - if (t.RotationZYX()) - { - _tfs.push_back(poseZ); - _tfs.push_back(poseY); - _tfs.push_back(poseX); - } - else if (t.RotationXYZ()) - { - _tfs.push_back(poseX); - _tfs.push_back(poseY); - _tfs.push_back(poseZ); - } - _tfs.push_back(poseT); + pose.Rot() = t.Rotation().value(); } + _tfs.push_back(pose); parent = parent.GetParent(); } @@ -297,12 +229,10 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) if (op == kXFormOpRotateZYX) { attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateZYX)); - t.SetRotationZYX(true); } else { attribute = _prim.GetAttribute(pxr::TfToken(kXFormOpRotateXYZ)); - t.SetRotationXYZ(true); } if (attribute.GetTypeName().GetCPPTypeName() == kGfVec3fString) { @@ -324,9 +254,11 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0); qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian()); - t.AddRotation(qX); - t.AddRotation(qY); - t.AddRotation(qZ); + if (op == kXFormOpRotateZYX) + { + std::swap(angleX, angleZ); + } + t.SetRotation((qX * qY) * qZ); } else if (op == kXFormOpTranslate) { @@ -370,7 +302,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) rotationQuad.GetImaginary()[0], rotationQuad.GetImaginary()[1], rotationQuad.GetImaginary()[2]); - t.AddRotation(q); + t.SetRotation(q); } if (op == kXFormOpTransform) @@ -397,7 +329,7 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) rotQuat.GetImaginary()[1], rotQuat.GetImaginary()[2] ); - t.AddRotation(q); + t.SetRotation(q); } } return t; diff --git a/usd/src/usd_parser/USDTransforms_TEST.cc b/usd/src/usd_parser/USDTransforms_TEST.cc index 179cd29aa..b6a5d9bfa 100644 --- a/usd/src/usd_parser/USDTransforms_TEST.cc +++ b/usd/src/usd_parser/USDTransforms_TEST.cc @@ -15,6 +15,8 @@ * */ +#include +#include #include #include @@ -39,10 +41,8 @@ void checkTransforms( const std::string &_primName, pxr::UsdStageRefPtr &_stage, const ignition::math::Vector3d &_translation, - const std::vector &_rotation, - const ignition::math::Vector3d &_scale, - bool isXYZRotation, - bool isZYXRotation) + const std::optional &_rotation, + const ignition::math::Vector3d &_scale) { pxr::UsdPrim prim = _stage->GetPrimAtPath(pxr::SdfPath(_primName)); ASSERT_TRUE(prim); @@ -52,21 +52,19 @@ void checkTransforms( EXPECT_EQ(_translation, usdTransforms.Translation()); EXPECT_EQ(_scale, usdTransforms.Scale()); - ASSERT_EQ(_rotation.size(), usdTransforms.Rotations().size()); - for (unsigned int i = 0; i < _rotation.size(); ++i) + if (_rotation) { - EXPECT_EQ(_rotation[i], usdTransforms.Rotations()[i]); + ASSERT_TRUE(usdTransforms.Rotation()); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Roll(), + usdTransforms.Rotation().value().Roll(), 0.1)); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Pitch(), + usdTransforms.Rotation().value().Pitch(), 0.1)); + EXPECT_TRUE(ignition::math::equal( + _rotation.value().Yaw(), + usdTransforms.Rotation().value().Yaw(), 0.1)); } - - EXPECT_EQ(!_rotation.empty(), usdTransforms.Rotation()); - EXPECT_EQ(isXYZRotation, usdTransforms.RotationXYZ()); - EXPECT_EQ(isZYXRotation, usdTransforms.RotationZYX()); - const bool hasRotation = isXYZRotation || isZYXRotation; - if (hasRotation) - { - EXPECT_NE(usdTransforms.RotationXYZ(), usdTransforms.RotationZYX()); - } - EXPECT_EQ(hasRotation, usdTransforms.Rotation()); } ///////////////////////////////////////////////// @@ -80,118 +78,75 @@ TEST(USDTransformsTest, ParseUSDTransform) "/shapes/ground_plane", stage, ignition::math::Vector3d(0, 0, -0.125), - { - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0) - }, - ignition::math::Vector3d(1, 1, 1), - true, - false + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Vector3d(1, 1, 1) ); checkTransforms( "/shapes/ground_plane/link/visual/geometry", stage, ignition::math::Vector3d(0, 0, 0), - {}, - ignition::math::Vector3d(100, 100, 0.25), - false, - false + std::nullopt, + ignition::math::Vector3d(100, 100, 0.25) ); checkTransforms( "/shapes/cylinder", stage, ignition::math::Vector3d(0, -1.5, 0.5), - { - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0) - }, - ignition::math::Vector3d(1, 1, 1), - true, - false + ignition::math::Quaterniond(1, 0, 0, 0), + ignition::math::Vector3d(1, 1, 1) ); checkTransforms( "/shapes/sphere", stage, ignition::math::Vector3d(0, 1.5, 0.5), - { - ignition::math::Quaterniond(IGN_DTOR(-69), 0, 0), - ignition::math::Quaterniond(0, IGN_DTOR(31), 0), - ignition::math::Quaterniond(0, 0, IGN_DTOR(-62)) - }, - ignition::math::Vector3d(1, 1, 1), - false, - true + ignition::math::Quaterniond( + IGN_DTOR(-62), IGN_DTOR(-47.5), IGN_DTOR(-53.41)), + ignition::math::Vector3d(1, 1, 1) ); checkTransforms( "/shapes/capsule", stage, ignition::math::Vector3d(0, -3.0, 0.5), - { - ignition::math::Quaterniond(IGN_DTOR(15), 0, 0), - ignition::math::Quaterniond(0, IGN_DTOR(80), 0), - ignition::math::Quaterniond(0, 0, IGN_DTOR(-55)) - }, - ignition::math::Vector3d(1, 1, 1), - false, - true + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)), + ignition::math::Vector3d(1, 1, 1) ); checkTransforms( "/shapes/capsule/capsule_link/capsule_visual", stage, ignition::math::Vector3d(0, 0, 0), - { - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(1, 0, 0, 0), - ignition::math::Quaterniond(0, 0, M_PI_2) - }, - ignition::math::Vector3d(1, 1, 1), - false, - true + ignition::math::Quaterniond(0, 0, M_PI_2), + ignition::math::Vector3d(1, 1, 1) ); checkTransforms( "/shapes/ellipsoid", stage, ignition::math::Vector3d(0, 3.0, 0.5), - { - ignition::math::Quaterniond(IGN_DTOR(15), 0, 0), - ignition::math::Quaterniond(0, IGN_DTOR(80), 0), - ignition::math::Quaterniond(0, 0, IGN_DTOR(-55)) - }, - ignition::math::Vector3d(1, 1, 1), - true, - false + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2)), + ignition::math::Vector3d(1, 1, 1) ); checkTransforms( "/shapes/ellipsoid/ellipsoid_link/ellipsoid_visual/geometry", stage, ignition::math::Vector3d(0, 0, 0), - {}, - ignition::math::Vector3d(0.4, 0.6, 1), - false, - false + std::nullopt, + ignition::math::Vector3d(0.4, 0.6, 1) ); checkTransforms( "/shapes/sun", stage, ignition::math::Vector3d(0, 0, 10), - { - ignition::math::Quaterniond(1, 0, 0, 0), ignition::math::Quaterniond(0, IGN_DTOR(-35), 0), - ignition::math::Quaterniond(1, 0, 0, 0) - }, - ignition::math::Vector3d(1, 1, 1), - true, - false + ignition::math::Vector3d(1, 1, 1) ); } @@ -220,7 +175,8 @@ TEST(USDTransformsTest, GetAllTransform) EXPECT_EQ( ignition::math::Pose3d( ignition::math::Vector3d(0, 0.03, 0.005), - ignition::math::Quaterniond(IGN_DTOR(15), IGN_DTOR(80), IGN_DTOR(-55))), + ignition::math::Quaterniond( + IGN_DTOR(-75.1), IGN_DTOR(49.2), IGN_DTOR(-81.2))), pose); } @@ -233,20 +189,33 @@ TEST(USDTransformsTest, GetAllTransform) sdf::usd::USDData usdData(filename); usdData.Init(); - const pxr::UsdPrim prim = stage->GetPrimAtPath( - pxr::SdfPath("/transforms/nested_transforms_XYZ/child_transform")); - ASSERT_TRUE(prim); - - ignition::math::Pose3d pose; - ignition::math::Vector3d scale{1, 1, 1}; - - sdf::usd::GetTransform(prim, usdData, pose, scale, "/transforms"); - - EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), scale); - EXPECT_EQ( - ignition::math::Pose3d( - ignition::math::Vector3d(.01, .01, 0), - ignition::math::Quaterniond(0, 0, IGN_DTOR(90))), - pose); + std::function verifyNestedTf = + [&](const std::string &_path, + const ignition::math::Vector3d &_posePrim, + const ignition::math::Quaterniond &_qPrim) + { + pxr::UsdPrim prim = stage->GetPrimAtPath(pxr::SdfPath(_path)); + ASSERT_TRUE(prim); + + ignition::math::Pose3d pose; + ignition::math::Vector3d scale{1, 1, 1}; + + sdf::usd::GetTransform(prim, usdData, pose, scale, "/transforms"); + + EXPECT_EQ(ignition::math::Vector3d(1, 1, 1), scale); + EXPECT_EQ(ignition::math::Pose3d(_posePrim, _qPrim), pose); + }; + + verifyNestedTf( + "/transforms/nested_transforms_XYZ/child_transform", + ignition::math::Vector3d(0.01, 0.01, 0), + ignition::math::Quaterniond(0, 0, IGN_DTOR(90))); + verifyNestedTf( + "/transforms/nested_transforms_ZYX/child_transform", + ignition::math::Vector3d(0.02, 0.0, 0), + ignition::math::Quaterniond(IGN_DTOR(90), 0, 0)); } } From 3bb44270ae290fa7d53ccae1e5aad6b995771462 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 31 Mar 2022 18:35:15 +0200 Subject: [PATCH 33/33] added TODO Signed-off-by: ahcorde --- usd/src/usd_parser/USDTransforms.cc | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/usd/src/usd_parser/USDTransforms.cc b/usd/src/usd_parser/USDTransforms.cc index 216394c86..7b30b0259 100644 --- a/usd/src/usd_parser/USDTransforms.cc +++ b/usd/src/usd_parser/USDTransforms.cc @@ -254,10 +254,13 @@ UDSTransforms ParseUSDTransform(const pxr::UsdPrim &_prim) qY = ignition::math::Quaterniond(0, angleY.Normalized().Radian(), 0); qZ = ignition::math::Quaterniond(0, 0, angleZ.Normalized().Radian()); - if (op == kXFormOpRotateZYX) - { - std::swap(angleX, angleZ); - } + // TODO(ahcorde) This part should be reviewed, revisit how rotateXYZ + // and rotateZYX are handle. + // Related issue https://github.com/ignitionrobotics/sdformat/issues/926 + // if (op == kXFormOpRotateZYX) + // { + // std::swap(angleX, angleZ); + // } t.SetRotation((qX * qY) * qZ); } else if (op == kXFormOpTranslate)