diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 45d59d66..289177b2 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -114,6 +116,8 @@ using pxr::TfCallContext; using pxr::Tf_PostErrorHelper; // clang-format on +using pxr::MjcPhysicsTokens; + using mujoco::usd::AddAttributeConnection; using mujoco::usd::AddPrimInherit; using mujoco::usd::AddPrimReference; @@ -175,6 +179,8 @@ class ModelWriter { SetLayerMetadata(data_, pxr::SdfFieldKeys->DefaultPrim, body_paths_[kWorldIndex].GetNameToken()); + WritePhysicsScene(); + // Author mesh scope + mesh prims to be referenced. WriteMeshes(); WriteMaterials(); @@ -304,6 +310,15 @@ class ModelWriter { SetAttributeDefault(data_, xform_op_order_path, order); } + template + void WriteUniformAttribute(const pxr::SdfPath &prim_path, + const pxr::SdfValueTypeName &value_type_name, + const pxr::TfToken &token, const T &value) { + pxr::SdfPath attr_path = CreateAttributeSpec( + data_, prim_path, token, value_type_name, pxr::SdfVariabilityUniform); + SetAttributeDefault(data_, attr_path, value); + } + void PrependToXformOpOrder(const pxr::SdfPath &prim_path, const pxr::VtArray &order) { auto xform_op_order_path = @@ -432,6 +447,185 @@ class ModelWriter { pxr::UsdGeomTokens->none); } + void WritePhysicsScene() { + pxr::SdfPath physics_scene_path = CreatePrimSpec( + data_, body_paths_[kWorldIndex], pxr::UsdPhysicsTokens->PhysicsScene, + pxr::UsdPhysicsTokens->PhysicsScene); + + ApplyApiSchema(data_, physics_scene_path, MjcPhysicsTokens->SceneAPI); + + const std::vector> + option_double_attributes = { + {MjcPhysicsTokens->mjcOptionTimestep, spec_->option.timestep}, + {MjcPhysicsTokens->mjcOptionTolerance, spec_->option.tolerance}, + {MjcPhysicsTokens->mjcOptionLs_tolerance, + spec_->option.ls_tolerance}, + {MjcPhysicsTokens->mjcOptionNoslip_tolerance, + spec_->option.noslip_tolerance}, + {MjcPhysicsTokens->mjcOptionCcd_tolerance, + spec_->option.ccd_tolerance}, + {MjcPhysicsTokens->mjcOptionApirate, spec_->option.apirate}, + {MjcPhysicsTokens->mjcOptionImpratio, spec_->option.impratio}, + {MjcPhysicsTokens->mjcOptionDensity, spec_->option.density}, + {MjcPhysicsTokens->mjcOptionViscosity, spec_->option.viscosity}, + {MjcPhysicsTokens->mjcOptionO_margin, spec_->option.o_margin}, + }; + for (const auto &[token, value] : option_double_attributes) { + WriteUniformAttribute(physics_scene_path, pxr::SdfValueTypeNames->Double, + token, value); + } + + const std::vector> option_int_attributes = { + {MjcPhysicsTokens->mjcOptionIterations, spec_->option.iterations}, + {MjcPhysicsTokens->mjcOptionLs_iterations, spec_->option.ls_iterations}, + {MjcPhysicsTokens->mjcOptionNoslip_iterations, + spec_->option.noslip_iterations}, + {MjcPhysicsTokens->mjcOptionCcd_iterations, + spec_->option.ccd_iterations}, + {MjcPhysicsTokens->mjcOptionSdf_iterations, + spec_->option.sdf_iterations}, + {MjcPhysicsTokens->mjcOptionSdf_initpoints, + spec_->option.sdf_initpoints}, + }; + for (const auto &[token, value] : option_int_attributes) { + WriteUniformAttribute(physics_scene_path, pxr::SdfValueTypeNames->Int, + token, value); + } + + pxr::SdfPath cone_attr = CreateAttributeSpec( + data_, physics_scene_path, MjcPhysicsTokens->mjcOptionCone, + pxr::SdfValueTypeNames->Token, pxr::SdfVariabilityUniform); + + switch (spec_->option.cone) { + case mjCONE_PYRAMIDAL: + SetAttributeDefault(data_, cone_attr, MjcPhysicsTokens->pyramidal); + break; + case mjCONE_ELLIPTIC: + SetAttributeDefault(data_, cone_attr, MjcPhysicsTokens->elliptic); + break; + default: + break; + } + + pxr::SdfPath jacobian_attr = CreateAttributeSpec( + data_, physics_scene_path, MjcPhysicsTokens->mjcOptionJacobian, + pxr::SdfValueTypeNames->Token, pxr::SdfVariabilityUniform); + + switch (spec_->option.jacobian) { + case mjJAC_AUTO: + SetAttributeDefault(data_, jacobian_attr, MjcPhysicsTokens->auto_); + break; + case mjJAC_DENSE: + SetAttributeDefault(data_, jacobian_attr, MjcPhysicsTokens->dense); + break; + case mjJAC_SPARSE: + SetAttributeDefault(data_, jacobian_attr, MjcPhysicsTokens->sparse); + break; + default: + break; + } + + pxr::SdfPath solver_attr = CreateAttributeSpec( + data_, physics_scene_path, MjcPhysicsTokens->mjcOptionSolver, + pxr::SdfValueTypeNames->Token, pxr::SdfVariabilityUniform); + switch (spec_->option.solver) { + case mjSOL_NEWTON: + SetAttributeDefault(data_, solver_attr, MjcPhysicsTokens->newton); + break; + case mjSOL_PGS: + SetAttributeDefault(data_, solver_attr, MjcPhysicsTokens->pgs); + break; + case mjSOL_CG: + SetAttributeDefault(data_, solver_attr, MjcPhysicsTokens->cg); + break; + default: + break; + } + + pxr::GfVec3d wind(spec_->option.wind[0], spec_->option.wind[1], + spec_->option.wind[2]); + WriteUniformAttribute(physics_scene_path, pxr::SdfValueTypeNames->Double3, + MjcPhysicsTokens->mjcOptionWind, wind); + + pxr::GfVec3d magnetic(spec_->option.magnetic[0], spec_->option.magnetic[1], + spec_->option.magnetic[2]); + WriteUniformAttribute(physics_scene_path, pxr::SdfValueTypeNames->Double3, + MjcPhysicsTokens->mjcOptionMagnetic, magnetic); + + pxr::VtArray o_solref(spec_->option.o_solref, + spec_->option.o_solref + 2); + WriteUniformAttribute(physics_scene_path, + pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcOptionO_solref, o_solref); + + pxr::VtArray o_solimp(spec_->option.o_solimp, + spec_->option.o_solimp + 5); + WriteUniformAttribute(physics_scene_path, + pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcOptionO_solimp, o_solimp); + + pxr::VtArray o_friction(spec_->option.o_friction, + spec_->option.o_friction + 5); + WriteUniformAttribute(physics_scene_path, + pxr::SdfValueTypeNames->DoubleArray, + MjcPhysicsTokens->mjcOptionO_friction, o_friction); + + pxr::SdfPath integrator_attr = CreateAttributeSpec( + data_, physics_scene_path, MjcPhysicsTokens->mjcOptionIntegrator, + pxr::SdfValueTypeNames->Token, pxr::SdfVariabilityUniform); + switch (spec_->option.integrator) { + case mjINT_EULER: + SetAttributeDefault(data_, integrator_attr, MjcPhysicsTokens->euler); + break; + case mjINT_RK4: + SetAttributeDefault(data_, integrator_attr, MjcPhysicsTokens->rk4); + break; + default: + break; + } + + auto create_flag_attr = [&](pxr::TfToken token, int flag, bool enable) { + int flags = + enable ? spec_->option.enableflags : spec_->option.disableflags; + bool value = enable ? (flags & flag) : !(flags & flag); + WriteUniformAttribute(physics_scene_path, pxr::SdfValueTypeNames->Bool, + token, value); + }; + + const std::vector> enable_flags = { + {MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD}, + {MjcPhysicsTokens->mjcFlagIsland, mjENBL_ISLAND}, + {MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV}, + {MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY}, + {MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE}, + {MjcPhysicsTokens->mjcFlagInvdiscrete, mjENBL_INVDISCRETE}}; + for (const auto &[token, flag] : enable_flags) { + create_flag_attr(token, flag, true); + } + + const std::vector> disable_flags = { + {MjcPhysicsTokens->mjcFlagConstraint, mjDSBL_CONSTRAINT}, + {MjcPhysicsTokens->mjcFlagEquality, mjDSBL_EQUALITY}, + {MjcPhysicsTokens->mjcFlagFrictionloss, mjDSBL_FRICTIONLOSS}, + {MjcPhysicsTokens->mjcFlagLimit, mjDSBL_LIMIT}, + {MjcPhysicsTokens->mjcFlagContact, mjDSBL_CONTACT}, + {MjcPhysicsTokens->mjcFlagPassive, mjDSBL_PASSIVE}, + {MjcPhysicsTokens->mjcFlagGravity, mjDSBL_GRAVITY}, + {MjcPhysicsTokens->mjcFlagClampctrl, mjDSBL_CLAMPCTRL}, + {MjcPhysicsTokens->mjcFlagWarmstart, mjDSBL_WARMSTART}, + {MjcPhysicsTokens->mjcFlagFilterparent, mjDSBL_FILTERPARENT}, + {MjcPhysicsTokens->mjcFlagActuation, mjDSBL_ACTUATION}, + {MjcPhysicsTokens->mjcFlagRefsafe, mjDSBL_REFSAFE}, + {MjcPhysicsTokens->mjcFlagSensor, mjDSBL_SENSOR}, + {MjcPhysicsTokens->mjcFlagMidphase, mjDSBL_MIDPHASE}, + {MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP}, + {MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET}, + {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD}}; + for (const auto &[token, flag] : disable_flags) { + create_flag_attr(token, flag, false); + } + } + void WriteMeshes() { // Create a scope for the meshes to keep things organized pxr::SdfPath scope_path = @@ -835,7 +1029,7 @@ class ModelWriter { pxr::SdfPath site_path = WriteSiteGeom(site, body_path); SetPrimPurpose(data_, site_path, pxr::UsdGeomTokens->guide); - ApplyApiSchema(data_, site_path, pxr::MjcPhysicsTokens->SiteAPI); + ApplyApiSchema(data_, site_path, MjcPhysicsTokens->SiteAPI); int site_id = mjs_getId(site->element); auto transform = MujocoPosQuatToTransform(&model_->site_pos[3 * site_id], @@ -1022,26 +1216,17 @@ class ModelWriter { (cam_sensorsize[0] / 2.f - cam_intrinsic[2]) : vertical_apperture * aspect_ratio; - pxr::SdfPath clipping_range_attr_path = CreateAttributeSpec( - data_, camera_path, pxr::UsdGeomTokens->clippingRange, - pxr::SdfValueTypeNames->Float2); - SetAttributeDefault(data_, clipping_range_attr_path, - pxr::GfVec2f(znear, zfar)); - pxr::SdfPath focal_length_attr_path = - CreateAttributeSpec(data_, camera_path, pxr::UsdGeomTokens->focalLength, - pxr::SdfValueTypeNames->Float); - SetAttributeDefault(data_, focal_length_attr_path, znear); - - pxr::SdfPath vertical_aperture_attr_path = CreateAttributeSpec( - data_, camera_path, pxr::UsdGeomTokens->verticalAperture, - pxr::SdfValueTypeNames->Float); - SetAttributeDefault(data_, vertical_aperture_attr_path, vertical_apperture); - - pxr::SdfPath horizontal_aperture_attr_path = CreateAttributeSpec( - data_, camera_path, pxr::UsdGeomTokens->horizontalAperture, - pxr::SdfValueTypeNames->Float); - SetAttributeDefault(data_, horizontal_aperture_attr_path, - horizontal_aperture); + WriteUniformAttribute(camera_path, pxr::SdfValueTypeNames->Float2, + pxr::UsdGeomTokens->clippingRange, + pxr::GfVec2f(znear, zfar)); + WriteUniformAttribute(camera_path, pxr::SdfValueTypeNames->Float, + pxr::UsdGeomTokens->focalLength, znear); + WriteUniformAttribute(camera_path, pxr::SdfValueTypeNames->Float, + pxr::UsdGeomTokens->verticalAperture, + vertical_apperture); + WriteUniformAttribute(camera_path, pxr::SdfValueTypeNames->Float, + pxr::UsdGeomTokens->horizontalAperture, + horizontal_aperture); } void WriteCameras(mjsBody *body) { @@ -1105,10 +1290,6 @@ class ModelWriter { } // Create XformOp attribute for body transform. - pxr::SdfPath xform_op_path = - CreateAttributeSpec(data_, body_path, kTokens->xformOpTransform, - pxr::SdfValueTypeNames->Matrix4d); - // Make sure to account for the parent since UsdPhysics doesn't support // nested bodies! auto parent_xform = body_xforms_[model_->body_parentid[body_id]]; @@ -1118,7 +1299,8 @@ class ModelWriter { MujocoPosQuatToTransform(&model_->body_pos[body_id * 3], &model_->body_quat[body_id * 4]) * parent_xform; - SetAttributeDefault(data_, xform_op_path, body_xforms_[body_id]); + WriteUniformAttribute(body_path, pxr::SdfValueTypeNames->Matrix4d, + kTokens->xformOpTransform, body_xforms_[body_id]); // Create XformOpOrder attribute for body transform order. // For us this is simply the transform we authored above. diff --git a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc new file mode 100644 index 00000000..534dcffe --- /dev/null +++ b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc @@ -0,0 +1,190 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// 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 "src/experimental/usd/mjcPhysics/sceneAPI.h" +#include "test/fixture.h" +#include +#include +#include +#include +#include +#include + +namespace mujoco { +namespace { + +using pxr::SdfPath; +using MjcPhysicsSceneTest = MujocoTest; +using testing::NotNull; + +// clang-format off +#define EXPECT_TYPE_USD_FALLBACK_EQ_MODEL_DEFAULT(type, usd_attr, mjc_attr) \ + { \ + type value; \ + mjc_phys_scene.Get##usd_attr##Attr().Get(&value); \ + EXPECT_EQ((mjtNum)value, default_model->opt.mjc_attr); \ + } + + +#define EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(usd_attr, mjc_attr) \ + EXPECT_TYPE_USD_FALLBACK_EQ_MODEL_DEFAULT(double, usd_attr, mjc_attr) + +#define EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(usd_attr, mjc_flag) \ + { \ + bool flag; \ + mjc_phys_scene.Get##usd_attr##Attr().Get(&flag); \ + EXPECT_NE(flag, default_model->opt.disableflags & (mjc_flag)); \ + } + +#define EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(usd_attr, mjc_flag) \ + { \ + bool flag; \ + mjc_phys_scene.Get##usd_attr##Attr().Get(&flag); \ + EXPECT_EQ(flag, default_model->opt.enableflags & (mjc_flag)); \ + } + + +#define EXPECT_INT_USD_FALLBACK_EQ_MODEL_DEFAULT(usd_attr, mjc_attr) \ + EXPECT_TYPE_USD_FALLBACK_EQ_MODEL_DEFAULT(int, usd_attr, mjc_attr) + +#define EXPECT_VEC3_USD_FALLBACK_EQ_MODEL_DEFAULT(usd_attr, mjc_attr) \ + { \ + pxr::GfVec3d value; \ + mjc_phys_scene.Get##usd_attr##Attr().Get(&value); \ + EXPECT_EQ(value[0], default_model->opt.mjc_attr[0]); \ + EXPECT_EQ(value[1], default_model->opt.mjc_attr[1]); \ + EXPECT_EQ(value[2], default_model->opt.mjc_attr[2]); \ + } + +#define EXPECT_TYPE_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(type, usd_attr, mjc_attr)\ + { \ + pxr::Vt##type##Array mjc_attr; \ + mjc_phys_scene.Get##usd_attr##Attr().Get(&mjc_attr); \ + EXPECT_THAT( \ + mjc_attr, \ + testing::ElementsAreArray(default_model->opt.mjc_attr) \ + ); \ + } + +#define EXPECT_REAL_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(usd_attr, mjc_attr)\ + EXPECT_TYPE_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(Double, usd_attr, mjc_attr) + +#define EXPECT_INT_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(usd_attr, mjc_attr)\ + EXPECT_TYPE_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(Int, usd_attr, mjc_attr) +// clang-format on + +TEST_F(MjcPhysicsSceneTest, TestDefaults) { + auto stage = pxr::UsdStage::CreateInMemory(); + + auto physics_scene = + pxr::UsdPhysicsScene::Define(stage, SdfPath("/World/PhysicsScene")); + + auto mjc_phys_scene = pxr::MjcPhysicsSceneAPI::Apply(physics_scene.GetPrim()); + + mjSpec* empty_spec = mj_makeSpec(); + mjModel* default_model = mj_compile(empty_spec, nullptr); + EXPECT_THAT(default_model, NotNull()); + + // Check that all the USD schema fallback values are the same + // as the model defaults. + // If this test is failing due to an update of defaults in Mujoco you need to + // update mjcPhysics/schema.usda. + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(Timestep, timestep); + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(ApiRate, apirate); + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(ImpRatio, impratio); + + EXPECT_VEC3_USD_FALLBACK_EQ_MODEL_DEFAULT(Wind, wind); + EXPECT_VEC3_USD_FALLBACK_EQ_MODEL_DEFAULT(Magnetic, magnetic); + + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(Density, density); + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(Viscosity, viscosity); + + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(OMargin, o_margin); + EXPECT_REAL_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(OSolRef, o_solref); + EXPECT_REAL_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(OSolImp, o_solimp); + EXPECT_REAL_ARR_USD_FALLBACK_EQ_MODEL_DEFAULT(OFriction, o_friction); + + EXPECT_INT_USD_FALLBACK_EQ_MODEL_DEFAULT(Iterations, iterations); + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(Tolerance, tolerance); + EXPECT_INT_USD_FALLBACK_EQ_MODEL_DEFAULT(LSIterations, ls_iterations); + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(LSTolerance, ls_tolerance); + EXPECT_INT_USD_FALLBACK_EQ_MODEL_DEFAULT(NoslipIterations, noslip_iterations); + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(NoslipTolerance, noslip_tolerance); + EXPECT_INT_USD_FALLBACK_EQ_MODEL_DEFAULT(CCDIterations, ccd_iterations); + EXPECT_REAL_USD_FALLBACK_EQ_MODEL_DEFAULT(CCDTolerance, ccd_tolerance); + EXPECT_INT_USD_FALLBACK_EQ_MODEL_DEFAULT(SDFIterations, sdf_iterations); + EXPECT_INT_USD_FALLBACK_EQ_MODEL_DEFAULT(SDFInitPoints, sdf_initpoints); + + // We store the actuator disable groups as an array of integers, but the + // model stores it as a bitmask. + pxr::VtIntArray actuator_group_disable; + mjc_phys_scene.GetActuatorGroupDisableAttr().Get(&actuator_group_disable); + int bitmask = 0; + for (int ind : actuator_group_disable) { + bitmask |= 1 << ind; + } + EXPECT_EQ(default_model->opt.disableactuator, bitmask); + + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(ConstraintFlag, + mjDSBL_CONSTRAINT); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(EqualityFlag, + mjDSBL_EQUALITY); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FrictionLossFlag, + mjDSBL_FRICTIONLOSS); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(LimitFlag, mjDSBL_LIMIT); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(ContactFlag, + mjDSBL_CONTACT); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(PassiveFlag, + mjDSBL_PASSIVE); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(GravityFlag, + mjDSBL_GRAVITY); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(ClampCtrlFlag, + mjDSBL_CLAMPCTRL); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(WarmStartFlag, + mjDSBL_WARMSTART); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FilterParentFlag, + mjDSBL_FILTERPARENT); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(ActuationFlag, + mjDSBL_ACTUATION); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(RefSafeFlag, + mjDSBL_REFSAFE); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(SensorFlag, mjDSBL_SENSOR); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MidPhaseFlag, + mjDSBL_MIDPHASE); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(NativeCCDFlag, + mjDSBL_NATIVECCD); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(EulerDampFlag, + mjDSBL_EULERDAMP); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(AutoResetFlag, + mjDSBL_AUTORESET); + + EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(OverrideFlag, + mjENBL_OVERRIDE); + EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(EnergyFlag, mjENBL_ENERGY); + EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FwdinvFlag, mjENBL_FWDINV); + EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(InvDiscreteFlag, + mjENBL_INVDISCRETE); + EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag, + mjENBL_MULTICCD); + EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(IslandFlag, mjENBL_ISLAND); + + mj_deleteModel(default_model); + mj_deleteSpec(empty_spec); +} + +} // namespace +} // namespace mujoco diff --git a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc index 8186af02..b1359540 100644 --- a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc +++ b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc @@ -19,9 +19,11 @@ #include #include "src/experimental/usd/mjcPhysics/sceneAPI.h" #include "src/experimental/usd/mjcPhysics/siteAPI.h" +#include "src/experimental/usd/mjcPhysics/tokens.h" #include "test/experimental/usd/test_utils.h" #include "test/fixture.h" #include +#include #include #include #include @@ -49,6 +51,7 @@ #include #include #include +#include #include PXR_NAMESPACE_OPEN_SCOPE @@ -63,6 +66,8 @@ namespace mujoco { namespace usd { namespace { +using pxr::MjcPhysicsSiteAPI; +using pxr::MjcPhysicsTokens; using pxr::SdfPath; using MjcfSdfFileFormatPluginTest = MujocoTest; @@ -499,6 +504,417 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestGeomsPrims) { pxr::GfVec3f(10.0, 20.0, 30.0)); } +static const pxr::SdfPath kPhysicsScenePrimPath("/test/PhysicsScene"); + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimTimestep) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionTimestep), + 0.005); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimCone) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionCone), + MjcPhysicsTokens->elliptic); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimWind) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionWind), + pxr::GfVec3d(1, 2, 3)); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimApirate) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionApirate), + 1.2); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimImpratio) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionImpratio), + 0.8); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimMagnetic) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionMagnetic), + pxr::GfVec3d(1, 2, 3)); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimDensity) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionDensity), + 1.2); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimViscosity) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionViscosity), + 0.8); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimO_margin) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionO_margin), + 0.001); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimO_solref) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionO_solref), + pxr::VtArray({0.1, 0.2})); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimO_solimp) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionO_solimp), + pxr::VtArray({0.1, 0.2, 0.3, 0.4, 0.5})); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimTolerance) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionTolerance), + 0.0012); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimLSTolerance) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionLs_tolerance), + 0.0034); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimNoslipTolerance) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionNoslip_tolerance), + 0.0056); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimCCDTolerance) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionCcd_tolerance), + 0.0078); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimOFriction) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionO_friction), + pxr::VtArray({0.1, 0.2, 0.3, 0.4, 0.5})); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimIntegrator) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionIntegrator), + MjcPhysicsTokens->rk4); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimJacobian) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionJacobian), + MjcPhysicsTokens->sparse); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimSolver) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionSolver), + MjcPhysicsTokens->cg); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimIterations) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionIterations), + 10); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimLSIterations) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionLs_iterations), + 20); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimNoslipIterations) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionNoslip_iterations), + 30); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimCCDIterations) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionCcd_iterations), + 40); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimSDFInitPoints) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionSdf_initpoints), + 50); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimSDFIterations) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + ExpectAttributeEqual(stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionSdf_iterations), + 60); +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimDisableFlags) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + const std::vector kFlags = { + MjcPhysicsTokens->mjcFlagConstraint, + MjcPhysicsTokens->mjcFlagEquality, + MjcPhysicsTokens->mjcFlagFrictionloss, + MjcPhysicsTokens->mjcFlagLimit, + MjcPhysicsTokens->mjcFlagContact, + MjcPhysicsTokens->mjcFlagPassive, + MjcPhysicsTokens->mjcFlagGravity, + MjcPhysicsTokens->mjcFlagClampctrl, + MjcPhysicsTokens->mjcFlagWarmstart, + MjcPhysicsTokens->mjcFlagFilterparent, + MjcPhysicsTokens->mjcFlagActuation, + MjcPhysicsTokens->mjcFlagRefsafe, + MjcPhysicsTokens->mjcFlagSensor, + MjcPhysicsTokens->mjcFlagMidphase, + MjcPhysicsTokens->mjcFlagNativeccd, + MjcPhysicsTokens->mjcFlagEulerdamp, + MjcPhysicsTokens->mjcFlagAutoreset, + }; + for (const auto& flag : kFlags) { + ExpectAttributeEqual(stage, kPhysicsScenePrimPath.AppendProperty(flag), + false); + } +} + +TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimEnableFlags) { + auto stage = pxr::UsdStage::Open(LoadLayer(R"( + + + + )")); + + // clang-format off + const std::vector kFlags = { + MjcPhysicsTokens->mjcFlagOverride, + MjcPhysicsTokens->mjcFlagEnergy, + MjcPhysicsTokens->mjcFlagFwdinv, + MjcPhysicsTokens->mjcFlagInvdiscrete, + MjcPhysicsTokens->mjcFlagMulticcd, + MjcPhysicsTokens->mjcFlagIsland, + }; + // clang-format on + for (const auto& flag : kFlags) { + ExpectAttributeEqual(stage, kPhysicsScenePrimPath.AppendProperty(flag), + true); + } +} + static constexpr char kSiteXml[] = R"( @@ -520,28 +936,28 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestSitePrimsAuthored) { auto stage = pxr::UsdStage::Open(layer); EXPECT_PRIM_VALID(stage, "/test/box_site"); EXPECT_PRIM_IS_A(stage, "/test/box_site", pxr::UsdGeomCube); - EXPECT_PRIM_API_APPLIED(stage, "/test/box_site", pxr::MjcPhysicsSiteAPI); + EXPECT_PRIM_API_APPLIED(stage, "/test/box_site", MjcPhysicsSiteAPI); EXPECT_PRIM_VALID(stage, "/test/ball/ball/sphere_site"); EXPECT_PRIM_IS_A(stage, "/test/ball/ball/sphere_site", pxr::UsdGeomSphere); EXPECT_PRIM_API_APPLIED(stage, "/test/ball/ball/sphere_site", - pxr::MjcPhysicsSiteAPI); + MjcPhysicsSiteAPI); EXPECT_PRIM_VALID(stage, "/test/ball/ball/capsule_site"); EXPECT_PRIM_IS_A(stage, "/test/ball/ball/capsule_site", pxr::UsdGeomCapsule); EXPECT_PRIM_API_APPLIED(stage, "/test/ball/ball/capsule_site", - pxr::MjcPhysicsSiteAPI); + MjcPhysicsSiteAPI); EXPECT_PRIM_VALID(stage, "/test/ball/ball/cylinder_site"); EXPECT_PRIM_IS_A(stage, "/test/ball/ball/cylinder_site", pxr::UsdGeomCylinder); EXPECT_PRIM_API_APPLIED(stage, "/test/ball/ball/cylinder_site", - pxr::MjcPhysicsSiteAPI); + MjcPhysicsSiteAPI); EXPECT_PRIM_VALID(stage, "/test/ball/ball/ellipsoid_site"); EXPECT_PRIM_IS_A(stage, "/test/ball/ball/ellipsoid_site", pxr::UsdGeomSphere); EXPECT_PRIM_API_APPLIED(stage, "/test/ball/ball/ellipsoid_site", - pxr::MjcPhysicsSiteAPI); + MjcPhysicsSiteAPI); } TEST_F(MjcfSdfFileFormatPluginTest, TestSitePrimsPurpose) { diff --git a/test/experimental/usd/test_utils.cc b/test/experimental/usd/test_utils.cc index 8bfc30e0..a4e93240 100644 --- a/test/experimental/usd/test_utils.cc +++ b/test/experimental/usd/test_utils.cc @@ -44,9 +44,9 @@ pxr::SdfLayerRefPtr LoadLayer( template <> void ExpectAttributeEqual(pxr::UsdStageRefPtr stage, - const char* path, + pxr::SdfPath path, const pxr::SdfAssetPath& value) { - auto attr = stage->GetAttributeAtPath(pxr::SdfPath(path)); + auto attr = stage->GetAttributeAtPath(path); EXPECT_TRUE(attr.IsValid()); pxr::SdfAssetPath attr_value; attr.Get(&attr_value); diff --git a/test/experimental/usd/test_utils.h b/test/experimental/usd/test_utils.h index ec5e3de7..69075ab3 100644 --- a/test/experimental/usd/test_utils.h +++ b/test/experimental/usd/test_utils.h @@ -74,7 +74,7 @@ pxr::SdfLayerRefPtr LoadLayer( const pxr::SdfFileFormat::FileFormatArguments& args = {}); template -void ExpectAttributeEqual(pxr::UsdStageRefPtr stage, const char* path, +void ExpectAttributeEqual(pxr::UsdStageRefPtr stage, pxr::SdfPath path, const T& value) { auto attr = stage->GetAttributeAtPath(pxr::SdfPath(path)); EXPECT_TRUE(attr.IsValid()) << "Attribute " << path << " is not valid"; @@ -84,13 +84,19 @@ void ExpectAttributeEqual(pxr::UsdStageRefPtr stage, const char* path, << attr_value << ". Expected: " << value; } +template +void ExpectAttributeEqual(pxr::UsdStageRefPtr stage, const char* path, + const T& value) { + ExpectAttributeEqual(stage, pxr::SdfPath(path), value); +} + // Specialization for SdfAssetPath, so that we can compare only the asset path // and not care about whatever the resolved path is. // Otherwise the default operator== would fail because it tests for equality of // the asset path AND the resolved path. template <> void ExpectAttributeEqual(pxr::UsdStageRefPtr stage, - const char* path, + pxr::SdfPath, const pxr::SdfAssetPath& value); void ExpectAttributeHasConnection(pxr::UsdStageRefPtr stage, const char* path,