From 0fa9e786507c2ac72c41a987e85dd2a02110b92e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 13 Apr 2022 19:03:51 +0200 Subject: [PATCH] USD to SDF: Added Joints (#899) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Co-authored-by: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> --- test/usd/double_pendulum.usda | 688 +++++++++++++++++++++++++++ usd/src/CMakeLists.txt | 7 + usd/src/usd_parser/USDJoints.cc | 283 +++++++++++ usd/src/usd_parser/USDJoints.hh | 56 +++ usd/src/usd_parser/USDJoints_TEST.cc | 91 ++++ usd/src/usd_parser/USDWorld.cc | 49 +- 6 files changed, 1169 insertions(+), 5 deletions(-) create mode 100644 test/usd/double_pendulum.usda create mode 100644 usd/src/usd_parser/USDJoints.cc create mode 100644 usd/src/usd_parser/USDJoints.hh create mode 100644 usd/src/usd_parser/USDJoints_TEST.cc diff --git a/test/usd/double_pendulum.usda b/test/usd/double_pendulum.usda new file mode 100644 index 000000000..2b175b5ff --- /dev/null +++ b/test/usd/double_pendulum.usda @@ -0,0 +1,688 @@ +#usda 1.0 +( + upAxis = "Z" +) + +def "double_pendulum" +{ + def PhysicsScene "physics" + { + } + + def Xform "ground_plane" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, -0.125) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "link" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 1 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "visual" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (100, 100, 0.25) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } + } + + def Xform "double_pendulum_with_base" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "base" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 100 + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "vis_plate_on_ground" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.01) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.8, -0.8, -0.01), (0.8, 0.8, 0.01)] + double height = 0.02 + rel material:binding = + double radius = 0.8 + } + } + + def Xform "vis_pole" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (-0.275, 0, 1.1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cube "geometry" + { + float3[] extent = [(-0.5, -0.5, -0.5), (0.5, 0.5, 0.5)] + rel material:binding = + double size = 1 + float3 xformOp:scale = (0.2, 0.2, 2.2) + uniform token[] xformOpOrder = ["xformOp:scale"] + } + } + } + + def Xform "upper_link" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 1 + float3 xformOp:rotateXYZ = (-90.00021, 0, 0) + double3 xformOp:translate = (0, 0, 2.1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "vis_upper_joint" + { + float3 xformOp:rotateXYZ = (180, 89.99979, 180) + double3 xformOp:translate = (-0.05, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.15), (0.1, 0.1, 0.15)] + double height = 0.3 + rel material:binding = + double radius = 0.1 + } + } + + def Xform "vis_lower_joint" + { + float3 xformOp:rotateXYZ = (180, 89.99979, 180) + double3 xformOp:translate = (0, 0, 1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.1), (0.1, 0.1, 0.1)] + double height = 0.2 + rel material:binding = + double radius = 0.1 + } + } + + def Xform "vis_cylinder" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.45), (0.1, 0.1, 0.45)] + double height = 0.9 + rel material:binding = + double radius = 0.1 + } + } + } + + def Xform "lower_link" ( + prepend apiSchemas = ["PhysicsMassAPI"] + ) + { + float physics:mass = 1 + float3 xformOp:rotateXYZ = (-114.59156, 0, 0) + double3 xformOp:translate = (0.25, 1, 2.1) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Xform "vis_lower_joint" + { + float3 xformOp:rotateXYZ = (180, 89.99979, 180) + double3 xformOp:translate = (0, 0, 0) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.08, -0.08, -0.15), (0.08, 0.08, 0.15)] + double height = 0.3 + rel material:binding = + double radius = 0.08 + } + } + + def Xform "vis_cylinder" + { + float3 xformOp:rotateXYZ = (0, 0, 0) + double3 xformOp:translate = (0, 0, 0.5) + uniform token[] xformOpOrder = ["xformOp:translate", "xformOp:rotateXYZ"] + + def Cylinder "geometry" + { + float3[] extent = [(-0.1, -0.1, -0.45), (0.1, 0.1, 0.45)] + double height = 0.9 + rel material:binding = + double radius = 0.1 + } + } + } + + def PhysicsRevoluteJoint "upper_joint" + { + uniform token physics:axis = "X" + prepend rel physics:body0 = + prepend rel physics:body1 = + point3f physics:localPos0 = (0, 0, 2.1) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0.70710546, -0.7071081, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -5.729578e17 + float physics:upperLimit = 5.729578e17 + } + + def PhysicsRevoluteJoint "lower" + { + uniform token physics:axis = "X" + prepend rel physics:body0 = + prepend rel physics:body1 = + point3f physics:localPos0 = (0.25, -0.0000036732051, 1) + point3f physics:localPos1 = (0, 0, 0) + quatf physics:localRot0 = (0.9770616, -0.21295662, 0, 0) + quatf physics:localRot1 = (1, 0, 0, 0) + float physics:lowerLimit = -5.729578e17 + float physics:upperLimit = 5.729578e17 + } + } +} + +def Scope "Looks" +{ + def Material "Material_0" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_1" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_2" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_3" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_4" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_5" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_6" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } + + def Material "Material_7" + { + token outputs:mdl:displacement.connect = + token outputs:mdl:surface.connect = + token outputs:mdl:volume.connect = + + def Shader "Shader" + { + uniform token info:implementationSource = "sourceAsset" + uniform asset info:mdl:sourceAsset = @OmniPBR.mdl@ + uniform token info:mdl:sourceAsset:subIdentifier = "OmniPBR" + color3f inputs:diffuse_color_constant = (0.8, 0.8, 0.8) ( + customData = { + float3 default = (0.2, 0.2, 0.2) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Albedo" + displayName = "Base Color" + doc = "This is the base color" + ) + color3f inputs:emissive_color = (0, 0, 0) ( + customData = { + float3 default = (1, 0.1, 0.1) + dictionary range = { + float3 max = (100000, 100000, 100000) + float3 min = (0, 0, 0) + } + } + displayGroup = "Emissive" + displayName = "Emissive Color" + doc = "The emission color" + ) + float inputs:emissive_intensity = 1 ( + customData = { + int default = 40 + dictionary range = { + int max = 100000 + int min = 0 + } + } + displayGroup = "Emissive" + displayName = "Emissive Intensity" + doc = "Intensity of the emission" + ) + bool inputs:enable_emission = 1 ( + customData = { + int default = 0 + } + displayGroup = "Emissive" + displayName = "Enable Emissive" + doc = "Enables the emission of light from the material" + ) + token outputs:out + } + } +} diff --git a/usd/src/CMakeLists.txt b/usd/src/CMakeLists.txt index bcc7c5d03..5f2279335 100644 --- a/usd/src/CMakeLists.txt +++ b/usd/src/CMakeLists.txt @@ -15,6 +15,7 @@ set(sources usd_parser/polygon_helper.cc usd_parser/USD2SDF.cc usd_parser/USDData.cc + usd_parser/USDJoints.cc usd_parser/USDLights.cc usd_parser/USDLinks.cc usd_parser/USDMaterial.cc @@ -51,6 +52,7 @@ set(gtest_sources sdf_parser/World_Sdf2Usd_TEST.cc usd_parser/USDData_TEST.cc usd_parser/USDPhysics_TEST.cc + usd_parser/USDJoints_TEST.cc usd_parser/USDLinks_TEST.cc usd_parser/USDLight_TEST.cc usd_parser/USDStage_TEST.cc @@ -75,6 +77,11 @@ if (TARGET UNIT_USDLinks_TEST) usd_parser/polygon_helper.cc) endif() +if (TARGET UNIT_USDJoints_TEST) + target_sources(UNIT_USDJoints_TEST PRIVATE + usd_parser/USDJoints.cc) +endif() + if (TARGET UNIT_USDPhysics_TEST) target_sources(UNIT_USDPhysics_TEST PRIVATE usd_parser/USDPhysics.cc) diff --git a/usd/src/usd_parser/USDJoints.cc b/usd/src/usd_parser/USDJoints.cc new file mode 100644 index 000000000..84b3a7818 --- /dev/null +++ b/usd/src/usd_parser/USDJoints.cc @@ -0,0 +1,283 @@ +/* + * 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 "USDJoints.hh" + +#include +#include +#include + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#include +#include +#include +#include +#pragma pop_macro ("__DEPRECATED") + +#include + +#include "sdf/usd/UsdError.hh" +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + UsdErrors ParseJoints( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + sdf::Joint &_joint) + { + UsdErrors errors; + + std::pair> usdData = + _usdData.FindStage(_prim.GetPath().GetName()); + double metersPerUnit = usdData.second->MetersPerUnit(); + + pxr::SdfPathVector body0, body1; + + auto variant_physics_joint = pxr::UsdPhysicsJoint(_prim); + + if (variant_physics_joint.GetBody0Rel()) + variant_physics_joint.GetBody0Rel().GetTargets(&body0); + if (variant_physics_joint.GetBody1Rel()) + variant_physics_joint.GetBody1Rel().GetTargets(&body1); + + if (body1.size() > 0) + { + _joint.SetChildLinkName(ignition::common::basename( + body1[0].GetString())); + } + else if (body0.size() > 0) + { + _joint.SetParentLinkName("world"); + _joint.SetChildLinkName(ignition::common::basename( + body0[0].GetString())); + } + + if (body0.size() > 0 && _joint.ParentLinkName().empty()) + { + _joint.SetParentLinkName(ignition::common::basename( + body0[0].GetString())); + } + else + { + _joint.SetParentLinkName("world"); + } + + std::string primName = _prim.GetName(); + if (primName.find("_joint") == std::string::npos) + { + _joint.SetName(std::string(_prim.GetName()) + "_joint"); + } + else + { + _joint.SetName(std::string(_prim.GetName())); + } + + float lowerLimit; + float upperLimit; + float stiffness; + float damping; + float maxForce; + float jointFriction; + float vel; + ignition::math::Quaterniond q1; + ignition::math::Quaterniond q2; + pxr::GfVec3f trans; + ignition::math::Vector3d axisVector; + sdf::JointAxis jointAxis; + + if (_prim.IsA() || + _prim.IsA()) + { + _joint.SetPoseRelativeTo(_joint.ParentLinkName()); + + pxr::TfToken axis; + if (_prim.IsA()) + { + pxr::UsdPhysicsPrismaticJoint(_prim).GetAxisAttr().Get(&axis); + } + else + { + pxr::UsdPhysicsRevoluteJoint(_prim).GetAxisAttr().Get(&axis); + } + + if (axis == pxr::UsdGeomTokens->x) + { + axisVector = ignition::math::Vector3d(1, 0, 0); + } + else if (axis == pxr::UsdGeomTokens->y) + { + axisVector = ignition::math::Vector3d(0, 1, 0); + } + else if (axis == pxr::UsdGeomTokens->z) + { + axisVector = ignition::math::Vector3d(0, 0, 1); + } + + pxr::GfVec3f localPose0, localPose1; + pxr::GfQuatf localRot0, localRot1; + + const auto usdPhysicsJoint = pxr::UsdPhysicsJoint(_prim); + usdPhysicsJoint.GetLocalPos0Attr().Get(&localPose0); + usdPhysicsJoint.GetLocalPos1Attr().Get(&localPose1); + usdPhysicsJoint.GetLocalRot0Attr().Get(&localRot0); + usdPhysicsJoint.GetLocalRot1Attr().Get(&localRot1); + + trans = (localPose0 + localPose1) * metersPerUnit; + + q1 = ignition::math::Quaterniond( + localRot0.GetReal(), + localRot0.GetImaginary()[0], + localRot0.GetImaginary()[1], + localRot0.GetImaginary()[2]); + q2 = ignition::math::Quaterniond( + localRot1.GetReal(), + localRot1.GetImaginary()[0], + localRot1.GetImaginary()[1], + localRot1.GetImaginary()[2]); + + _prim.GetAttribute( + pxr::TfToken("physics:lowerLimit")).Get(&lowerLimit); + _prim.GetAttribute( + pxr::TfToken("physics:upperLimit")).Get(&upperLimit); + if (_prim.IsA()) + { + _prim.GetAttribute( + pxr::TfToken("drive:linear:physics:stiffness")).Get(&stiffness); + _prim.GetAttribute( + pxr::TfToken("drive:linear:physics:damping")).Get(&damping); + _prim.GetAttribute( + pxr::TfToken("drive:linear:physics:maxForce")).Get(&maxForce); + } + else + { + _prim.GetAttribute( + pxr::TfToken("drive:angular:physics:stiffness")).Get(&stiffness); + _prim.GetAttribute( + pxr::TfToken("drive:angular:physics:damping")).Get(&damping); + _prim.GetAttribute( + pxr::TfToken("drive:angular:physics:maxForce")).Get(&maxForce); + } + _prim.GetAttribute( + pxr::TfToken("physxJoint:maxJointVelocity")).Get(&vel); + + pxr::UsdAttribute jointFrictionAttribute; + if (jointFrictionAttribute = _prim.GetAttribute( + pxr::TfToken("physxJoint:jointFriction"))) + { + jointFrictionAttribute.Get(&jointFriction); + } + else if (jointFrictionAttribute = _prim.GetAttribute( + pxr::TfToken("jointFriction"))) + { + jointFrictionAttribute.Get(&jointFriction); + } + + jointAxis.SetDamping(damping); + jointAxis.SetEffort(maxForce); + jointAxis.SetSpringStiffness(stiffness); + jointAxis.SetFriction(jointFriction); + jointAxis.SetMaxVelocity(vel); + } + + if (_prim.IsA()) + { + auto variant_physics_prismatic_joint = + pxr::UsdPhysicsPrismaticJoint(_prim); + + _joint.SetType(sdf::JointType::PRISMATIC); + + auto errorsAxis = jointAxis.SetXyz(-(q2 * axisVector).Round()); + if (!errorsAxis.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Errors encountered when setting xyz of prismatic " + "joint axis: [" + std::string(_prim.GetName()) + "]")); + for (const auto & error : errorsAxis) + errors.emplace_back(error); + return errors; + } + + _joint.SetRawPose( + ignition::math::Pose3d( + ignition::math::Vector3d(trans[0], trans[1], trans[2]), + ignition::math::Quaterniond(q1 * q2))); + + jointAxis.SetLower(lowerLimit * metersPerUnit); + jointAxis.SetUpper(upperLimit * metersPerUnit); + _joint.SetAxis(0, jointAxis); + + return errors; + } + else if (_prim.IsA()) + { + auto variant_physics_revolute_joint = + pxr::UsdPhysicsRevoluteJoint(_prim); + + _joint.SetType(sdf::JointType::REVOLUTE); + + auto errorsAxis = jointAxis.SetXyz(axisVector); + if (!errorsAxis.empty()) + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Errors encountered when setting xyz of revolute " + "joint axis: [" + std::string(_prim.GetName()) + "]")); + for (const auto & error : errorsAxis) + errors.emplace_back(error); + return errors; + } + + _joint.SetRawPose(ignition::math::Pose3d( + ignition::math::Vector3d(trans[0], trans[1], trans[2]), + q1)); + + jointAxis.SetLower(IGN_DTOR(lowerLimit)); + jointAxis.SetUpper(IGN_DTOR(upperLimit)); + _joint.SetAxis(0, jointAxis); + + return errors; + } + else if (_prim.IsA() || + _prim.IsA()) + { + _joint.SetType(sdf::JointType::FIXED); + } + else + { + errors.emplace_back(UsdError( + sdf::usd::UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to create a SDF joint from USD prim [" + + std::string(_prim.GetName()) + + "] because the prim is not a USD joint.")); + } + + return errors; + } + } + } +} diff --git a/usd/src/usd_parser/USDJoints.hh b/usd/src/usd_parser/USDJoints.hh new file mode 100644 index 000000000..9a635700a --- /dev/null +++ b/usd/src/usd_parser/USDJoints.hh @@ -0,0 +1,56 @@ +/* + * 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_JOINTS_HH +#define USD_PARSER_JOINTS_HH + +#pragma push_macro ("__DEPRECATED") +#undef __DEPRECATED +#include +#pragma pop_macro ("__DEPRECATED") + +#include "sdf/Joint.hh" +#include "sdf/usd/usd_parser/USDData.hh" +#include "sdf/config.hh" + +namespace sdf +{ + // Inline bracket to help doxygen filtering. + inline namespace SDF_VERSION_NAMESPACE { + // + namespace usd + { + /// \brief Parse a USD joint to its SDF representation + /// This method will parse revolute and prismatic joints, if the Joint type + /// is not one of these two, then the joint is fixed. This will help to + /// avoid issues with when two or more links links in a model doesn't have + /// a joint + /// + /// \param[in] _prim The USD prim that holds the USD joint + /// \param[in] _usdData Object that holds data about the USD stage + /// \param[out] _joint SDF joint to return + /// \return UsdErrors, which is a list of UsdError objects. An empty list + /// means there were no errors parsing joint + UsdErrors ParseJoints( + const pxr::UsdPrim &_prim, + const USDData &_usdData, + sdf::Joint &_joint); + } + } +} + +#endif diff --git a/usd/src/usd_parser/USDJoints_TEST.cc b/usd/src/usd_parser/USDJoints_TEST.cc new file mode 100644 index 000000000..1d79e78f6 --- /dev/null +++ b/usd/src/usd_parser/USDJoints_TEST.cc @@ -0,0 +1,91 @@ +/* + * 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 + +// 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 + +#include "test_config.h" +#include "test_utils.hh" + +#include "USDJoints.hh" + +#include "sdf/Joint.hh" +#include "sdf/JointAxis.hh" +#include "sdf/usd/usd_parser/USDData.hh" + +///////////////////////////////////////////////// +TEST(USDJointTest, JointTest) +{ + const std::string filename = + sdf::testing::TestFile("usd", "double_pendulum.usda"); + const auto stage = pxr::UsdStage::Open(filename); + ASSERT_TRUE(stage); + + sdf::usd::USDData usdData(filename); + usdData.Init(); + + const auto upperJoint = stage->GetPrimAtPath(pxr::SdfPath( + "/double_pendulum/double_pendulum_with_base/upper_joint")); + ASSERT_TRUE(upperJoint); + + sdf::Joint joint1; + auto errors = sdf::usd::ParseJoints( + upperJoint, usdData, joint1); + + EXPECT_EQ(0u, errors.size()); + EXPECT_EQ(sdf::JointType::REVOLUTE, joint1.Type()); + EXPECT_EQ("upper_joint", joint1.Name()); + EXPECT_EQ("upper_link", joint1.ChildLinkName()); + EXPECT_EQ("base", joint1.ParentLinkName()); + EXPECT_EQ(ignition::math::Pose3d(0, 0, 0.021, -1.5708, 0, 0), + joint1.RawPose()); + + auto axis = joint1.Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_EQ(ignition::math::Vector3d(1, 0, 0), axis->Xyz()); + + const auto lowerJoint = stage->GetPrimAtPath(pxr::SdfPath( + "/double_pendulum/double_pendulum_with_base/lower")); + ASSERT_TRUE(lowerJoint); + + sdf::Joint joint2; + errors = sdf::usd::ParseJoints( + lowerJoint, usdData, joint2); + + EXPECT_EQ(0u, errors.size()); + EXPECT_EQ(sdf::JointType::REVOLUTE, joint2.Type()); + EXPECT_EQ("lower_joint", joint2.Name()); + EXPECT_EQ("lower_link", joint2.ChildLinkName()); + EXPECT_EQ("upper_link", joint2.ParentLinkName()); + EXPECT_EQ(ignition::math::Pose3d(0.0025, -0, 0.01, -0.4292, 0, 0), + joint2.RawPose()); + axis = joint2.Axis(0); + ASSERT_NE(nullptr, axis); + EXPECT_EQ(ignition::math::Vector3d(1, 0, 0), axis->Xyz()); +} diff --git a/usd/src/usd_parser/USDWorld.cc b/usd/src/usd_parser/USDWorld.cc index c2a1cacdd..7fcdf7d8b 100644 --- a/usd/src/usd_parser/USDWorld.cc +++ b/usd/src/usd_parser/USDWorld.cc @@ -27,14 +27,15 @@ #pragma push_macro ("__DEPRECATED") #undef __DEPRECATED +#include #include #include #include #include #include -#include #include #include +#include #include #pragma pop_macro ("__DEPRECATED") @@ -42,14 +43,17 @@ #include "sdf/usd/usd_parser/USDStage.hh" #include "sdf/usd/usd_parser/USDTransforms.hh" +#include "USDJoints.hh" #include "USDLights.hh" #include "USDPhysics.hh" #include "USDLinks.hh" +#include "sdf/Collision.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" #include "sdf/Model.hh" #include "sdf/Plugin.hh" +#include "sdf/Visual.hh" #include "sdf/World.hh" namespace sdf @@ -183,6 +187,43 @@ namespace usd } // TODO(anyone) support converting other USD light types + sdf::Model *modelPtr = nullptr; + if (!currentModelName.empty()) + { + modelPtr = _world.ModelByName(currentModelName); + if (!modelPtr) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to find a sdf::Model named [" + currentModelName + + "] in world named [" + _world.Name() + + "], but a sdf::Model with this name should exist.")); + return errors; + } + } + + if (prim.IsA()) + { + if (!modelPtr) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to parse joint corresponding to USD prim [" + + std::string(prim.GetName()) + + "] because the corresponding sdf::Model object wasn't found.")); + return errors; + } + sdf::Joint joint; + auto errorsJoint = ParseJoints(prim, usdData, joint); + if (!errorsJoint.empty()) + { + errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, + "Unable to find parse UsdPhysicsJoint [" + + std::string(prim.GetName()) + "]")); + return errors; + } + modelPtr->AddJoint(joint); + continue; + } + if (prim.IsA()) { std::pair> data = @@ -205,13 +246,11 @@ namespace usd continue; } - auto modelPtr = _world.ModelByName(currentModelName); if (!modelPtr) { errors.push_back(UsdError(UsdErrorCode::USD_TO_SDF_PARSING_ERROR, - "Unable to find a sdf::Model named [" + currentModelName + - "] in world named [" + _world.Name() + - "], but a sdf::Model with this name should exist.")); + "Unable to parse link named [" + linkName + + "] because the corresponding sdf::Model object wasn't found.")); return errors; }