diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index 58a24590e4..87fdfe879d 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,27 @@ using pxr::MjcPhysicsTokens; using pxr::TfToken; +template +using TfStaticData = pxr::TfStaticData; + +// clang-format off +TF_DEFINE_PRIVATE_TOKENS(kNewtonTokens, + ((NewtonMaterialAPI, "NewtonMaterialAPI")) + ((NewtonMeshCollisionAPI, "NewtonMeshCollisionAPI")) + ((newtonMaxSolverIterations, "newton:maxSolverIterations")) + ((newtonTimeStepsPerSecond, "newton:timeStepsPerSecond")) + ((newtonGravityEnabled, "newton:gravityEnabled")) + ((newtonContactMargin, "newton:contactMargin")) + ((newtonContactGap, "newton:contactGap")) + ((newtonMaxHullVertices, "newton:maxHullVertices")) + ((newtonTorsionalFriction, "newton:torsionalFriction")) + ((newtonRollingFriction, "newton:rollingFriction")) + ((newtonMimicJoint, "newton:mimicJoint")) + ((newtonMimicCoef0, "newton:mimicCoef0")) + ((newtonMimicCoef1, "newton:mimicCoef1")) + ((NewtonMimicAPI, "NewtonMimicAPI")) +); +// clang-format on struct UsdCaches { pxr::UsdGeomXformCache xform_cache; @@ -463,15 +485,48 @@ void ParseUsdPhysicsScene(mjSpec* spec, SetGravityAttributes(spec, stage, gravity_direction, gravity_magnitude); - // Early exit if theres no MjcPhysicsSceneAPI applied. - if (!physics_scene.GetPrim().HasAPI()) { + // Parse Newton scene attributes if present (works for Newton-only files) + pxr::UsdPrim scene_prim = physics_scene.GetPrim(); + auto newton_iterations = scene_prim.GetAttribute( + kNewtonTokens->newtonMaxSolverIterations); + if (newton_iterations && newton_iterations.HasAuthoredValue()) { + int val; + newton_iterations.Get(&val); + if (val >= 0) spec->option.iterations = val; + } + auto newton_timesteps = scene_prim.GetAttribute( + kNewtonTokens->newtonTimeStepsPerSecond); + if (newton_timesteps && newton_timesteps.HasAuthoredValue()) { + int val; + newton_timesteps.Get(&val); + if (val > 0) spec->option.timestep = 1.0 / val; + } + auto newton_gravity = scene_prim.GetAttribute( + kNewtonTokens->newtonGravityEnabled); + if (newton_gravity && newton_gravity.HasAuthoredValue()) { + bool enabled; + newton_gravity.Get(&enabled); + if (!enabled) { + spec->option.disableflags |= mjDSBL_GRAVITY; + } + } + + // Early exit if there's no MjcPhysicsSceneAPI applied. + if (!scene_prim.HasAPI()) { return; } - auto mjc_physics_scene = pxr::MjcPhysicsSceneAPI(physics_scene.GetPrim()); + auto mjc_physics_scene = pxr::MjcPhysicsSceneAPI(scene_prim); - double timestep; - mjc_physics_scene.GetTimestepAttr().Get(×tep); - spec->option.timestep = timestep; + // MJC values override Newton values only when explicitly authored. + auto timestep_attr = mjc_physics_scene.GetTimestepAttr(); + if (timestep_attr.HasAuthoredValue()) { + double timestep; + timestep_attr.Get(×tep); + spec->option.timestep = timestep; + mju_warning("Scene '%s' uses deprecated mjc:option:timestep. " + "Please migrate to newton:timeStepsPerSecond.", + scene_prim.GetPath().GetText()); + } double impratio; mjc_physics_scene.GetImpRatioAttr().Get(&impratio); @@ -586,9 +641,15 @@ void ParseUsdPhysicsScene(mjSpec* spec, spec->option.solver = mjSOL_PGS; } - int iterations; - mjc_physics_scene.GetIterationsAttr().Get(&iterations); - spec->option.iterations = iterations; + auto iterations_attr = mjc_physics_scene.GetIterationsAttr(); + if (iterations_attr.HasAuthoredValue()) { + int iterations; + iterations_attr.Get(&iterations); + spec->option.iterations = iterations; + mju_warning("Scene '%s' uses deprecated mjc:option:iterations. " + "Please migrate to newton:maxSolverIterations.", + scene_prim.GetPath().GetText()); + } int ls_iterations; mjc_physics_scene.GetLSIterationsAttr().Get(&ls_iterations); @@ -638,9 +699,19 @@ void ParseUsdPhysicsScene(mjSpec* spec, mjc_physics_scene.GetDamperFlagAttr().Get(&damper_flag); spec->option.disableflags |= (!damper_flag ? mjDSBL_DAMPER : 0); - bool gravity_flag; - mjc_physics_scene.GetGravityFlagAttr().Get(&gravity_flag); - spec->option.disableflags |= (!gravity_flag ? mjDSBL_GRAVITY : 0); + auto gravity_flag_attr = mjc_physics_scene.GetGravityFlagAttr(); + if (gravity_flag_attr.HasAuthoredValue()) { + bool gravity_flag; + gravity_flag_attr.Get(&gravity_flag); + if (!gravity_flag) { + spec->option.disableflags |= mjDSBL_GRAVITY; + } else { + spec->option.disableflags &= ~mjDSBL_GRAVITY; + } + mju_warning("Scene '%s' uses deprecated mjc:flag:gravity. " + "Please migrate to newton:gravityEnabled.", + scene_prim.GetPath().GetText()); + } bool clampctrl_flag; mjc_physics_scene.GetClampCtrlFlagAttr().Get(&clampctrl_flag); @@ -914,13 +985,39 @@ void ParseMjcPhysicsCollisionAPI( } auto margin_attr = collision_api.GetMarginAttr(); - if (margin_attr.HasAuthoredValue()) { + auto gap_attr = collision_api.GetGapAttr(); + bool mjc_margin_authored = margin_attr.HasAuthoredValue(); + bool mjc_gap_authored = gap_attr.HasAuthoredValue(); + + if (mjc_margin_authored) { margin_attr.Get(&geom->margin); + mju_warning("Prim '%s' uses deprecated mjc:margin. " + "Please migrate to newton:contactMargin and newton:contactGap.", + collision_api.GetPrim().GetPath().GetText()); } - - auto gap_attr = collision_api.GetGapAttr(); - if (gap_attr.HasAuthoredValue()) { + if (mjc_gap_authored) { gap_attr.Get(&geom->gap); + mju_warning("Prim '%s' uses deprecated mjc:gap. " + "Please migrate to newton:contactGap.", + collision_api.GetPrim().GetPath().GetText()); + } + + // Newton collision fallback: newton:contactMargin + newton:contactGap -> margin, gap + if (!mjc_margin_authored || !mjc_gap_authored) { + pxr::UsdPrim prim = collision_api.GetPrim(); + auto newton_margin = prim.GetAttribute(kNewtonTokens->newtonContactMargin); + auto newton_gap = prim.GetAttribute(kNewtonTokens->newtonContactGap); + float n_margin = 0, n_gap = 0; + bool has_newton_margin = newton_margin && newton_margin.HasAuthoredValue(); + bool has_newton_gap = newton_gap && newton_gap.HasAuthoredValue(); + if (has_newton_margin) newton_margin.Get(&n_margin); + if (has_newton_gap) newton_gap.Get(&n_gap); + if (!mjc_gap_authored && has_newton_gap) { + geom->gap = n_gap; + } + if (!mjc_margin_authored && has_newton_margin) { + geom->margin = n_margin + geom->gap; + } } } @@ -942,8 +1039,19 @@ void ParseMjcPhysicsMeshCollisionAPI( } auto maxhullvert_attr = mesh_collision_api.GetMaxHullVertAttr(); + auto newton_maxhull = mesh_collision_api.GetPrim().GetAttribute( + kNewtonTokens->newtonMaxHullVertices); if (maxhullvert_attr.HasAuthoredValue()) { maxhullvert_attr.Get(&mesh->maxhullvert); + if (!newton_maxhull || !newton_maxhull.HasAuthoredValue()) { + mju_warning("Prim '%s' uses deprecated mjc:maxhullvert. " + "Please migrate to newton:maxHullVertices.", + mesh_collision_api.GetPrim().GetPath().GetText()); + } + } else if (newton_maxhull && newton_maxhull.HasAuthoredValue()) { + int val; + newton_maxhull.Get(&val); + mesh->maxhullvert = val; } } @@ -1668,15 +1776,42 @@ void ParseUsdPhysicsMaterialAPI( } void ParseMjcPhysicsMaterialAPI( - mjsGeom* geom, const pxr::MjcPhysicsMaterialAPI& material_api) { - auto torsional_friction_attr = material_api.GetTorsionalFrictionAttr(); - if (torsional_friction_attr.HasAuthoredValue()) { - torsional_friction_attr.Get(&geom->friction[1]); - } - - auto rolling_friction_attr = material_api.GetRollingFrictionAttr(); - if (rolling_friction_attr.HasAuthoredValue()) { - rolling_friction_attr.Get(&geom->friction[2]); + mjsGeom* geom, const pxr::UsdPrim& material_prim, + const pxr::MjcPhysicsMaterialAPI& material_api) { + // Torsional friction: prefer newton:torsionalFriction, fall back to + // mjc:torsionalfriction with deprecation warning. If both are authored, + // mjc takes precedence for backwards compatibility. + auto mjc_torsional = material_api.GetTorsionalFrictionAttr(); + auto newton_torsional = material_prim.GetAttribute( + kNewtonTokens->newtonTorsionalFriction); + if (mjc_torsional.HasAuthoredValue()) { + mjc_torsional.Get(&geom->friction[1]); + if (!newton_torsional || !newton_torsional.HasAuthoredValue()) { + mju_warning("Prim '%s' uses deprecated mjc:torsionalfriction. " + "Please migrate to newton:torsionalFriction.", + material_prim.GetPath().GetText()); + } + } else if (newton_torsional && newton_torsional.HasAuthoredValue()) { + float val; + newton_torsional.Get(&val); + geom->friction[1] = val; + } + + // Rolling friction: same deprecation/fallback pattern. + auto mjc_rolling = material_api.GetRollingFrictionAttr(); + auto newton_rolling = material_prim.GetAttribute( + kNewtonTokens->newtonRollingFriction); + if (mjc_rolling.HasAuthoredValue()) { + mjc_rolling.Get(&geom->friction[2]); + if (!newton_rolling || !newton_rolling.HasAuthoredValue()) { + mju_warning("Prim '%s' uses deprecated mjc:rollingfriction. " + "Please migrate to newton:rollingFriction.", + material_prim.GetPath().GetText()); + } + } else if (newton_rolling && newton_rolling.HasAuthoredValue()) { + float val; + newton_rolling.Get(&val); + geom->friction[2] = val; } } @@ -1789,11 +1924,13 @@ void ParseUsdPhysicsCollider(mjSpec* spec, if (bound_material) { pxr::UsdPrim bound_material_prim = bound_material.GetPrim(); if (bound_material_prim.HasAPI() || - bound_material_prim.HasAPI()) { + bound_material_prim.HasAPI() || + bound_material_prim.HasAPI(kNewtonTokens->NewtonMaterialAPI)) { ParseUsdPhysicsMaterialAPI( geom, pxr::UsdPhysicsMaterialAPI(bound_material_prim)); ParseMjcPhysicsMaterialAPI( - geom, pxr::MjcPhysicsMaterialAPI(bound_material_prim)); + geom, bound_material_prim, + pxr::MjcPhysicsMaterialAPI(bound_material_prim)); } pxr::SdfPath material_path = bound_material_prim.GetPath(); mjsMaterial* material = nullptr; @@ -1826,7 +1963,9 @@ void ParseUsdPhysicsCollider(mjSpec* spec, if (!MaybeParseGeomPrimitive(prim, geom, caches.xform_cache)) { mjsMesh* mesh = ParseUsdMesh(spec, prim, geom, caches.xform_cache); - if (mesh != nullptr && prim.HasAPI()) { + if (mesh != nullptr && + (prim.HasAPI() || + prim.HasAPI(kNewtonTokens->NewtonMeshCollisionAPI))) { ParseMjcPhysicsMeshCollisionAPI(mesh, pxr::MjcPhysicsMeshCollisionAPI(prim)); } @@ -1877,8 +2016,8 @@ void ParseMjcEqualityAPISolverParams( void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, pxr::UsdGeomXformCache& xform_cache) { - if (prim.HasAPI()) { - // Handle MjcPhysicsEqualityJointAPI on revolute/prismatic joints. + if (prim.HasAPI() || + prim.HasAPI(kNewtonTokens->NewtonMimicAPI)) { pxr::MjcPhysicsEqualityJointAPI eq_joint_api(prim); mjsEquality* eq = mjs_addEquality(spec, nullptr); eq->type = mjEQ_JOINT; @@ -1889,20 +2028,48 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, eq->objtype = mjOBJ_JOINT; mjs_setString(eq->name1, prim.GetPath().GetAsString().c_str()); - // Get the target joint (joint2) from the MjcEqualityAPI target - // relationship. - pxr::MjcPhysicsEqualityAPI equality_api(prim); - pxr::UsdRelationship target_rel = equality_api.GetMjcTargetRel(); + // Target joint: prefer newton:mimicJoint, fall back to deprecated mjc:target pxr::SdfPathVector targets; - target_rel.GetTargets(&targets); - if (!targets.empty()) { + auto newton_mimic_rel = prim.GetRelationship(kNewtonTokens->newtonMimicJoint); + if (newton_mimic_rel && newton_mimic_rel.GetTargets(&targets) && !targets.empty()) { mjs_setString(eq->name2, targets[0].GetAsString().c_str()); + } else { + auto mjc_target_rel = prim.GetRelationship(MjcPhysicsTokens->mjcTarget); + if (mjc_target_rel && mjc_target_rel.GetTargets(&targets) && !targets.empty()) { + mjs_setString(eq->name2, targets[0].GetAsString().c_str()); + mju_warning("Prim '%s' uses deprecated mjc:target. " + "Please migrate to newton:mimicJoint.", + prim.GetPath().GetText()); + } } - // If no target, name2 remains empty, meaning joint1 is fixed to a constant. - // Parse individual coefficient attributes for the quartic polynomial. - eq_joint_api.GetCoef0Attr().Get(&eq->data[0]); - eq_joint_api.GetCoef1Attr().Get(&eq->data[1]); + // Coefficients: prefer Newton, fall back to deprecated MJC + auto newton_coef0 = prim.GetAttribute(kNewtonTokens->newtonMimicCoef0); + auto newton_coef1 = prim.GetAttribute(kNewtonTokens->newtonMimicCoef1); + if (newton_coef0 && newton_coef0.HasAuthoredValue()) { + float val; + newton_coef0.Get(&val); + eq->data[0] = val; + } else { + eq_joint_api.GetCoef0Attr().Get(&eq->data[0]); + if (eq_joint_api.GetCoef0Attr().HasAuthoredValue()) { + mju_warning("Prim '%s' uses deprecated mjc:coef0. " + "Please migrate to newton:mimicCoef0.", + prim.GetPath().GetText()); + } + } + if (newton_coef1 && newton_coef1.HasAuthoredValue()) { + float val; + newton_coef1.Get(&val); + eq->data[1] = val; + } else { + eq_joint_api.GetCoef1Attr().Get(&eq->data[1]); + if (eq_joint_api.GetCoef1Attr().HasAuthoredValue()) { + mju_warning("Prim '%s' uses deprecated mjc:coef1. " + "Please migrate to newton:mimicCoef1.", + prim.GetPath().GetText()); + } + } eq_joint_api.GetCoef2Attr().Get(&eq->data[2]); eq_joint_api.GetCoef3Attr().Get(&eq->data[3]); eq_joint_api.GetCoef4Attr().Get(&eq->data[4]); @@ -1910,7 +2077,27 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, pxr::UsdPhysicsJoint joint(prim); ParseJointEnabled(eq, joint); - ParseMjcEqualityAPISolverParams(eq, equality_api, prim); + // Solver params are now inline on MjcEqualityJointAPI + auto solref_attr = prim.GetAttribute(MjcPhysicsTokens->mjcSolref); + if (solref_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solref; + solref_attr.Get(&solref); + if (solref.size() == mjNREF) { + for (int i = 0; i < mjNREF; ++i) { + eq->solref[i] = solref[i]; + } + } + } + auto solimp_attr = prim.GetAttribute(MjcPhysicsTokens->mjcSolimp); + if (solimp_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solimp; + solimp_attr.Get(&solimp); + if (solimp.size() == mjNIMP) { + for (int i = 0; i < mjNIMP; ++i) { + eq->solimp[i] = solimp[i]; + } + } + } } else if (prim.IsA() || prim.IsA()) { // Handle fixed joints as weld constraints, spherical joints as connect constraints diff --git a/src/experimental/usd/CMakeLists.txt b/src/experimental/usd/CMakeLists.txt index c2763ef37a..6db73264bb 100644 --- a/src/experimental/usd/CMakeLists.txt +++ b/src/experimental/usd/CMakeLists.txt @@ -199,6 +199,42 @@ target_link_libraries(mujoco PUBLIC ${MJC_PHYSICS_PLUGIN_TARGET_NAME} ) +## ----- Newton USD Schemas (codeless plugin) ----- + +function(install_newton_usd_plugin install_base_dir) + include(FetchContent) + + FetchContent_Declare( + newton-usd-schemas + GIT_REPOSITORY https://github.com/newton-physics/newton-usd-schemas.git + GIT_TAG v0.1.0rc3 + GIT_SHALLOW TRUE + UPDATE_DISCONNECTED TRUE + ) + + FetchContent_GetProperties(newton-usd-schemas) + if(NOT newton-usd-schemas_POPULATED) + FetchContent_Populate(newton-usd-schemas) + endif() + + set(NEWTON_USD_DIR "${newton-usd-schemas_SOURCE_DIR}/newton_usd_schemas") + if(NOT EXISTS "${NEWTON_USD_DIR}") + message(FATAL_ERROR "newton_usd_schemas directory not found in fetched repository") + endif() + + set(NEWTON_BUILD_DIR "${CMAKE_BINARY_DIR}/${install_base_dir}/newton") + file(MAKE_DIRECTORY "${NEWTON_BUILD_DIR}") + configure_file("${NEWTON_USD_DIR}/plugInfo.json" "${NEWTON_BUILD_DIR}/plugInfo.json" COPYONLY) + configure_file("${NEWTON_USD_DIR}/generatedSchema.usda" "${NEWTON_BUILD_DIR}/generatedSchema.usda" COPYONLY) + + install(FILES "${NEWTON_BUILD_DIR}/plugInfo.json" + DESTINATION "${install_base_dir}/newton" + ) + install(FILES "${NEWTON_BUILD_DIR}/generatedSchema.usda" + DESTINATION "${install_base_dir}/newton" + ) +endfunction() + ## Installation # Generate and install plugInfo.json for each plugin @@ -219,6 +255,10 @@ install(FILES DESTINATION ${MJ_USD_INSTALL_DIR_LIB}/mjcPhysics ) +install_newton_usd_plugin( + ${MJ_USD_INSTALL_DIR_LIB} +) + # Install shared libraries install(TARGETS ${MJCF_PLUGIN_TARGET_NAME} diff --git a/src/experimental/usd/mjcPhysics/generatedSchema.usda b/src/experimental/usd/mjcPhysics/generatedSchema.usda index 633ec10d5b..dec480b009 100644 --- a/src/experimental/usd/mjcPhysics/generatedSchema.usda +++ b/src/experimental/usd/mjcPhysics/generatedSchema.usda @@ -4,6 +4,7 @@ ) class "MjcSceneAPI" ( + apiSchemas = ["NewtonSceneAPI"] doc = "API providing global simulation options for MuJoCo." ) { @@ -120,7 +121,9 @@ class "MjcSceneAPI" ( ) uniform bool mjc:flag:gravity = 1 ( displayName = "Gravity Toggle" - doc = "Enables the application of gravitational acceleration as defined in mjOption." + doc = """DEPRECATED: Use newton:gravityEnabled instead. + + Enables the application of gravitational acceleration as defined in mjOption.""" ) uniform bool mjc:flag:invdiscrete = 0 ( displayName = "Discrete-Time Inverse Dynamics Toggle" @@ -200,7 +203,9 @@ class "MjcSceneAPI" ( ) uniform int mjc:option:iterations = 100 ( displayName = "Solver Iterations" - doc = "Maximum number of iterations of the constraint solver." + doc = """DEPRECATED: Use newton:maxSolverIterations instead. + + Maximum number of iterations of the constraint solver.""" ) uniform token mjc:option:jacobian = "auto" ( allowedTokens = ["auto", "dense", "sparse"] @@ -265,7 +270,9 @@ class "MjcSceneAPI" ( ) uniform double mjc:option:timestep = 0.002 ( displayName = "Timestep" - doc = "Controls the timestep in seconds used by MuJoCo." + doc = """DEPRECATED: Use newton:timeStepsPerSecond instead. + + Controls the timestep in seconds used by MuJoCo.""" ) uniform double mjc:option:tolerance = 1e-8 ( displayName = "Solver Tolerance" @@ -303,6 +310,7 @@ class "MjcImageableAPI" ( } class "MjcCollisionAPI" ( + apiSchemas = ["NewtonCollisionAPI"] doc = "API describing a MuJoCo collider." ) { @@ -312,7 +320,9 @@ class "MjcCollisionAPI" ( ) uniform double mjc:gap = 0 ( displayName = "Gap" - doc = "This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is positive, geom distances between margin and margin-gap correspond to such inactive contacts." + doc = """DEPRECATED: Use newton:contactGap instead. + + This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is positive, geom distances between margin and margin-gap correspond to such inactive contacts.""" ) uniform int mjc:group = 0 ( displayName = "Group" @@ -320,7 +330,9 @@ class "MjcCollisionAPI" ( ) uniform double mjc:margin = 0 ( displayName = "Margin" - doc = "Distance threshold below which contacts are detected and included in the global array mjData.contact." + doc = """DEPRECATED: Use newton:contactMargin and newton:contactGap instead. + + Distance threshold below which contacts are detected and included in the global array mjData.contact.""" ) uniform int mjc:priority = 0 ( displayName = "Priority" @@ -345,6 +357,7 @@ class "MjcCollisionAPI" ( } class "MjcMeshCollisionAPI" ( + apiSchemas = ["NewtonMeshCollisionAPI"] doc = "API describing a MuJoCo mesh collider." ) { @@ -355,7 +368,9 @@ class "MjcMeshCollisionAPI" ( ) uniform int mjc:maxhullvert = -1 ( displayName = "Maximum Hull Vertices" - doc = "Sets an upper limit on the number of vertices in the meshes convex hull. The default value of -1 means unlimited." + doc = """DEPRECATED: Use newton:maxHullVertices instead. + + Sets an upper limit on the number of vertices in the meshes convex hull. The default value of -1 means unlimited.""" ) } @@ -537,16 +552,23 @@ class "MjcJointAPI" ( } class "MjcMaterialAPI" ( - doc = "API providing extension attributes to represent physical MuJoCo materials." + apiSchemas = ["NewtonMaterialAPI"] + doc = """DEPRECATED: Use NewtonMaterialAPI instead. All attributes on this API have been superseded by Newton equivalents. + + API providing extension attributes to represent physical MuJoCo materials.""" ) { uniform double mjc:rollingfriction = 0.0001 ( displayName = "Rolling Friction" - doc = "Friction value acting around both axes on the contact tangent plane." + doc = """DEPRECATED: Use newton:rollingFriction instead. + + Friction value acting around both axes on the contact tangent plane.""" ) uniform double mjc:torsionalfriction = 0.005 ( displayName = "Torsional Friction" - doc = "Friction value acting around contact normal." + doc = """DEPRECATED: Use newton:torsionalFriction instead. + + Friction value acting around contact normal.""" ) } @@ -586,23 +608,36 @@ class "MjcEqualityWeldAPI" ( } class "MjcEqualityJointAPI" ( - apiSchemas = ["MjcEqualityAPI"] + apiSchemas = ["NewtonMimicAPI"] doc = """API providing extension attributes to represent equality/joint constraints. - This API is applied to a joint prim which acts as the constrained joint (joint1 in - MuJoCo terminology). The target relationship points to another joint prim which is - the reference joint (joint2 in MuJoCo terminology). The constrained joint's position - or angle is constrained to be a quartic polynomial of the reference joint's position - or angle. Only scalar joint types (slide and hinge) can be used.""" + + This API is applied to a joint prim which acts as the follower (joint0). The leader + joint (joint1) is specified via the newton:mimicJoint relationship inherited from + NewtonMimicAPI. + + The follower's position or angle is constrained to be a quartic polynomial of the + leader's position or angle: + joint0 = coef0 + coef1*(joint1) + coef2*(joint1)^2 + coef3*(joint1)^3 + coef4*(joint1)^4 + + The constant (coef0) and linear (coef1) coefficients are provided by NewtonMimicAPI + as newton:mimicCoef0 and newton:mimicCoef1. The higher-order coefficients (coef2-coef4) + are provided by this API. + + Only scalar joint types (slide and hinge) can be used.""" ) { uniform double mjc:coef0 = 0 ( displayName = "Coefficient 0" - doc = """Constant coefficient a0 of the quartic polynomial. The constraint is: + doc = """DEPRECATED: Use newton:mimicCoef0 instead. + + Constant coefficient a0 of the quartic polynomial. The constraint is: y = y0 + a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4.""" ) uniform double mjc:coef1 = 1 ( displayName = "Coefficient 1" - doc = "Linear coefficient a1 of the quartic polynomial." + doc = """DEPRECATED: Use newton:mimicCoef1 instead. + + Linear coefficient a1 of the quartic polynomial.""" ) uniform double mjc:coef2 = 0 ( displayName = "Coefficient 2" @@ -616,6 +651,19 @@ class "MjcEqualityJointAPI" ( displayName = "Coefficient 4" doc = "Quartic coefficient a4 of the quartic polynomial." ) + uniform double[] mjc:solimp = [0.9, 0.95, 0.001, 0.5, 2] ( + displayName = "SolImp" + doc = "Constraint solver parameter for equality constraint simulation." + ) + uniform double[] mjc:solref = [0.02, 1] ( + displayName = "SolRef" + doc = "Constraint solver parameter for equality constraint simulation." + ) + rel mjc:target ( + doc = """DEPRECATED: Use newton:mimicJoint instead. + + Secondary target of the equality constraint (the leader/reference joint).""" + ) } class MjcTendon "MjcTendon" ( diff --git a/src/experimental/usd/mjcPhysics/schema.usda b/src/experimental/usd/mjcPhysics/schema.usda index 62c1d993cc..dd705a6ead 100644 --- a/src/experimental/usd/mjcPhysics/schema.usda +++ b/src/experimental/usd/mjcPhysics/schema.usda @@ -100,6 +100,7 @@ class "MjcSceneAPI" } doc = """API providing global simulation options for MuJoCo.""" + prepend apiSchemas = ["NewtonSceneAPI"] inherits = ) { @@ -108,7 +109,9 @@ class "MjcSceneAPI" string apiName = "Timestep" } displayName = "Timestep" - doc = """Controls the timestep in seconds used by MuJoCo.""" + doc = """DEPRECATED: Use newton:timeStepsPerSecond instead. + + Controls the timestep in seconds used by MuJoCo.""" ) uniform double mjc:option:impratio = 1.0 ( @@ -229,7 +232,9 @@ class "MjcSceneAPI" string apiName = "Iterations" } displayName = "Solver Iterations" - doc = """Maximum number of iterations of the constraint solver.""" + doc = """DEPRECATED: Use newton:maxSolverIterations instead. + + Maximum number of iterations of the constraint solver.""" ) uniform double mjc:option:tolerance = 1e-08 ( @@ -378,7 +383,9 @@ class "MjcSceneAPI" string apiName = "GravityFlag" } displayName = "Gravity Toggle" - doc = """Enables the application of gravitational acceleration as defined in mjOption.""" + doc = """DEPRECATED: Use newton:gravityEnabled instead. + + Enables the application of gravitational acceleration as defined in mjOption.""" ) uniform bool mjc:flag:clampctrl = True ( @@ -674,6 +681,7 @@ class "MjcCollisionAPI" } doc = """API describing a MuJoCo collider.""" + prepend apiSchemas = ["NewtonCollisionAPI"] inherits = ) { @@ -738,7 +746,9 @@ class "MjcCollisionAPI" string apiName = "Margin" } displayName = "Margin" - doc = """Distance threshold below which contacts are detected and included in the global array mjData.contact.""" + doc = """DEPRECATED: Use newton:contactMargin and newton:contactGap instead. + + Distance threshold below which contacts are detected and included in the global array mjData.contact.""" ) uniform double mjc:gap = 0.0 ( @@ -746,7 +756,9 @@ class "MjcCollisionAPI" string apiName = "Gap" } displayName = "Gap" - doc = """This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is positive, geom distances between margin and margin-gap correspond to such inactive contacts.""" + doc = """DEPRECATED: Use newton:contactGap instead. + + This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is positive, geom distances between margin and margin-gap correspond to such inactive contacts.""" ) } @@ -757,6 +769,7 @@ class "MjcMeshCollisionAPI" } doc = """API describing a MuJoCo mesh collider.""" + prepend apiSchemas = ["NewtonMeshCollisionAPI"] inherits = ) { @@ -774,7 +787,9 @@ class "MjcMeshCollisionAPI" string apiName = "MaxHullVert" } displayName = "Maximum Hull Vertices" - doc = """Sets an upper limit on the number of vertices in the meshes convex hull. The default value of -1 means unlimited.""" + doc = """DEPRECATED: Use newton:maxHullVertices instead. + + Sets an upper limit on the number of vertices in the meshes convex hull. The default value of -1 means unlimited.""" ) } @@ -1035,8 +1050,11 @@ class "MjcMaterialAPI" customData = { string className = "MaterialAPI" } - doc = """API providing extension attributes to represent physical MuJoCo materials.""" + doc = """DEPRECATED: Use NewtonMaterialAPI instead. All attributes on this API have been superseded by Newton equivalents. + + API providing extension attributes to represent physical MuJoCo materials.""" + prepend apiSchemas = ["NewtonMaterialAPI"] inherits = ) { @@ -1045,7 +1063,9 @@ class "MjcMaterialAPI" string apiName = "TorsionalFriction" } displayName = "Torsional Friction" - doc = """Friction value acting around contact normal.""" + doc = """DEPRECATED: Use newton:torsionalFriction instead. + + Friction value acting around contact normal.""" ) uniform double mjc:rollingfriction = 0.0001 ( @@ -1053,7 +1073,9 @@ class "MjcMaterialAPI" string apiName = "RollingFriction" } displayName = "Rolling Friction" - doc = """Friction value acting around both axes on the contact tangent plane.""" + doc = """DEPRECATED: Use newton:rollingFriction instead. + + Friction value acting around both axes on the contact tangent plane.""" ) } @@ -1121,22 +1143,58 @@ class "MjcEqualityJointAPI" ( string className = "EqualityJointAPI" } doc = """API providing extension attributes to represent equality/joint constraints. - This API is applied to a joint prim which acts as the constrained joint (joint1 in - MuJoCo terminology). The target relationship points to another joint prim which is - the reference joint (joint2 in MuJoCo terminology). The constrained joint's position - or angle is constrained to be a quartic polynomial of the reference joint's position - or angle. Only scalar joint types (slide and hinge) can be used.""" - prepend apiSchemas = ["MjcEqualityAPI"] + This API is applied to a joint prim which acts as the follower (joint0). The leader + joint (joint1) is specified via the newton:mimicJoint relationship inherited from + NewtonMimicAPI. + + The follower's position or angle is constrained to be a quartic polynomial of the + leader's position or angle: + joint0 = coef0 + coef1*(joint1) + coef2*(joint1)^2 + coef3*(joint1)^3 + coef4*(joint1)^4 + + The constant (coef0) and linear (coef1) coefficients are provided by NewtonMimicAPI + as newton:mimicCoef0 and newton:mimicCoef1. The higher-order coefficients (coef2-coef4) + are provided by this API. + + Only scalar joint types (slide and hinge) can be used.""" + + prepend apiSchemas = ["NewtonMimicAPI"] inherits = ) { + uniform double[] mjc:solimp = [0.9, 0.95, 0.001, 0.5, 2] ( + customData = { + string apiName = "SolImp" + } + displayName = "SolImp" + doc = """Constraint solver parameter for equality constraint simulation.""" + ) + + uniform double[] mjc:solref = [0.02, 1] ( + customData = { + string apiName = "SolRef" + } + displayName = "SolRef" + doc = """Constraint solver parameter for equality constraint simulation.""" + ) + + rel mjc:target ( + customData = { + string apiName = "MjcTarget" + } + doc = """DEPRECATED: Use newton:mimicJoint instead. + + Secondary target of the equality constraint (the leader/reference joint).""" + ) + uniform double mjc:coef0 = 0 ( customData = { string apiName = "Coef0" } displayName = "Coefficient 0" - doc = """Constant coefficient a0 of the quartic polynomial. The constraint is: + doc = """DEPRECATED: Use newton:mimicCoef0 instead. + + Constant coefficient a0 of the quartic polynomial. The constraint is: y = y0 + a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4.""" ) @@ -1145,7 +1203,9 @@ class "MjcEqualityJointAPI" ( string apiName = "Coef1" } displayName = "Coefficient 1" - doc = """Linear coefficient a1 of the quartic polynomial.""" + doc = """DEPRECATED: Use newton:mimicCoef1 instead. + + Linear coefficient a1 of the quartic polynomial.""" ) uniform double mjc:coef2 = 0 ( diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 2820a3aa3b..71612246b8 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -15,6 +15,7 @@ #include "mjcf/mujoco_to_usd.h" #include +#include #include #include #include @@ -119,6 +120,16 @@ TF_DEFINE_PRIVATE_TOKENS(kTokens, (UsdPrimvarReader_float2) (UsdUVTexture) (UsdPreviewSurface) + ((NewtonMaterialAPI, "NewtonMaterialAPI")) + ((NewtonMeshCollisionAPI, "NewtonMeshCollisionAPI")) + ((newtonTorsionalFriction, "newton:torsionalFriction")) + ((newtonRollingFriction, "newton:rollingFriction")) + ((newtonMaxHullVertices, "newton:maxHullVertices")) + ((newtonMaxSolverIterations, "newton:maxSolverIterations")) + ((newtonTimeStepsPerSecond, "newton:timeStepsPerSecond")) + ((newtonGravityEnabled, "newton:gravityEnabled")) + ((newtonContactMargin, "newton:contactMargin")) + ((newtonContactGap, "newton:contactGap")) ); // Using to satisfy TF_REGISTRY_FUNCTION macro below and avoid operating in PXR_NS. @@ -393,8 +404,11 @@ class ModelWriter { WriteUniformAttribute(mesh_spec, pxr::SdfValueTypeNames->Token, MjcPhysicsTokens->mjcInertia, inertia); - WriteUniformAttribute(mesh_spec, pxr::SdfValueTypeNames->Int, - MjcPhysicsTokens->mjcMaxhullvert, mesh->maxhullvert); + // Newton mesh attribute (replaces deprecated mjc:maxhullvert) + if (mesh->maxhullvert != -1) { + WriteUniformAttribute(mesh_spec, pxr::SdfValueTypeNames->Int, + kTokens->newtonMaxHullVertices, mesh->maxhullvert); + } // NOTE: The geometry data taken from the spec is the post-compilation // data after it has been mjCMesh::Compile'd. So don't be surprised if @@ -500,7 +514,7 @@ class ModelWriter { const std::vector> option_double_attributes = { - {MjcPhysicsTokens->mjcOptionTimestep, spec_->option.timestep}, + // mjc:option:timestep deprecated in favor of newton:timeStepsPerSecond {MjcPhysicsTokens->mjcOptionTolerance, spec_->option.tolerance}, {MjcPhysicsTokens->mjcOptionLs_tolerance, spec_->option.ls_tolerance}, @@ -519,7 +533,7 @@ class ModelWriter { } const std::vector> option_int_attributes = { - {MjcPhysicsTokens->mjcOptionIterations, spec_->option.iterations}, + // mjc:option:iterations deprecated in favor of newton:maxSolverIterations {MjcPhysicsTokens->mjcOptionLs_iterations, spec_->option.ls_iterations}, {MjcPhysicsTokens->mjcOptionNoslip_iterations, spec_->option.noslip_iterations}, @@ -666,7 +680,7 @@ class ModelWriter { {MjcPhysicsTokens->mjcFlagContact, mjDSBL_CONTACT}, {MjcPhysicsTokens->mjcFlagSpring, mjDSBL_SPRING}, {MjcPhysicsTokens->mjcFlagDamper, mjDSBL_DAMPER}, - {MjcPhysicsTokens->mjcFlagGravity, mjDSBL_GRAVITY}, + // mjc:flag:gravity deprecated in favor of newton:gravityEnabled {MjcPhysicsTokens->mjcFlagClampctrl, mjDSBL_CLAMPCTRL}, {MjcPhysicsTokens->mjcFlagWarmstart, mjDSBL_WARMSTART}, {MjcPhysicsTokens->mjcFlagFilterparent, mjDSBL_FILTERPARENT}, @@ -747,6 +761,19 @@ class ModelWriter { WriteUniformAttribute(physics_scene_spec, pxr::SdfValueTypeNames->Bool, MjcPhysicsTokens->mjcCompilerSaveInertial, (bool)spec_->compiler.saveinertial); + + // Newton scene attributes (auto-applied via MjcSceneAPI -> NewtonSceneAPI) + WriteUniformAttribute(physics_scene_spec, pxr::SdfValueTypeNames->Int, + kTokens->newtonMaxSolverIterations, + spec_->option.iterations); + if (spec_->option.timestep > 0) { + WriteUniformAttribute(physics_scene_spec, pxr::SdfValueTypeNames->Int, + kTokens->newtonTimeStepsPerSecond, + static_cast(std::round(1.0 / spec_->option.timestep))); + } + bool gravity_disabled = spec_->option.disableflags & mjDSBL_GRAVITY; + WriteUniformAttribute(physics_scene_spec, pxr::SdfValueTypeNames->Bool, + kTokens->newtonGravityEnabled, !gravity_disabled); } void WriteMeshes() { @@ -868,16 +895,13 @@ class ModelWriter { pxr::UsdPhysicsTokens->physicsDynamicFriction, (float)geom->friction[0]); } - if (geom->friction[1] != geom_default->friction[1]) { - WriteUniformAttribute(material_spec, pxr::SdfValueTypeNames->Double, - MjcPhysicsTokens->mjcTorsionalfriction, - geom->friction[1]); - } - if (geom->friction[2] != geom_default->friction[2]) { - WriteUniformAttribute(material_spec, pxr::SdfValueTypeNames->Double, - MjcPhysicsTokens->mjcRollingfriction, - geom->friction[2]); - } + // Newton material attributes (replaces deprecated mjc:torsionalfriction / mjc:rollingfriction) + WriteUniformAttribute(material_spec, pxr::SdfValueTypeNames->Float, + kTokens->newtonTorsionalFriction, + (float)geom->friction[1]); + WriteUniformAttribute(material_spec, pxr::SdfValueTypeNames->Float, + kTokens->newtonRollingFriction, + (float)geom->friction[2]); return material_spec; } @@ -1706,11 +1730,13 @@ class ModelWriter { MjcPhysicsTokens->mjcSolimp, pxr::VtArray(geom->solimp, geom->solimp + mjNIMP)); - WriteUniformAttribute(geom_spec, pxr::SdfValueTypeNames->Double, - MjcPhysicsTokens->mjcMargin, geom->margin); - - WriteUniformAttribute(geom_spec, pxr::SdfValueTypeNames->Double, - MjcPhysicsTokens->mjcGap, geom->gap); + // Newton collision attributes (replaces deprecated mjc:margin / mjc:gap) + WriteUniformAttribute(geom_spec, pxr::SdfValueTypeNames->Float, + kTokens->newtonContactMargin, + static_cast(geom->margin - geom->gap)); + WriteUniformAttribute(geom_spec, pxr::SdfValueTypeNames->Float, + kTokens->newtonContactGap, + static_cast(geom->gap)); if (geom->mass >= mjMINVAL || geom->density >= mjMINVAL) { ApplyApiSchema(layer_, geom_spec, pxr::UsdPhysicsTokens->PhysicsMassAPI); 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 b74ebcff5a..d77ae85168 100644 --- a/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc +++ b/test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc @@ -78,6 +78,9 @@ PXR_NAMESPACE_OPEN_SCOPE // clang-format off TF_DEFINE_PRIVATE_TOKENS(_tokens, (st) + ((newtonTimeStepsPerSecond, "newton:timeStepsPerSecond")) + ((newtonMaxSolverIterations, "newton:maxSolverIterations")) + ((newtonGravityEnabled, "newton:gravityEnabled")) ); // clang-format on PXR_NAMESPACE_CLOSE_SCOPE @@ -159,12 +162,20 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsMaterials) { 4.0f); ExpectAttributeEqual(stage, "/physics_materials_test/PhysicsMaterials/" - "geom_with_friction.mjc:torsionalfriction", - 5.0); + "geom_with_friction.newton:torsionalFriction", + 5.0f); ExpectAttributeEqual(stage, "/physics_materials_test/PhysicsMaterials/" - "geom_with_friction.mjc:rollingfriction", - 6.0); + "geom_with_friction.newton:rollingFriction", + 6.0f); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, + "/physics_materials_test/PhysicsMaterials/" + "geom_with_friction.mjc:torsionalfriction"); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, + "/physics_materials_test/PhysicsMaterials/" + "geom_with_friction.mjc:rollingfriction"); } TEST_F(MjcfSdfFileFormatPluginTest, TestMaterials) { @@ -692,10 +703,15 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimTimestep) { )"); + // newton:timeStepsPerSecond = round(1/0.005) = 200 ExpectAttributeEqual( stage, - kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionTimestep), - 0.005); + kPhysicsScenePrimPath.AppendProperty(pxr::_tokens->newtonTimeStepsPerSecond), + 200); + // deprecated mjc:option:timestep should not be authored + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, + kPhysicsScenePrimPath.AppendProperty(MjcPhysicsTokens->mjcOptionTimestep)); } TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimCone) { @@ -928,8 +944,12 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimIterations) { ExpectAttributeEqual(stage, kPhysicsScenePrimPath.AppendProperty( - MjcPhysicsTokens->mjcOptionIterations), + pxr::_tokens->newtonMaxSolverIterations), 10); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, + kPhysicsScenePrimPath.AppendProperty( + MjcPhysicsTokens->mjcOptionIterations)); } TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimLSIterations) { @@ -1066,7 +1086,7 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimDisableFlags) { MjcPhysicsTokens->mjcFlagContact, MjcPhysicsTokens->mjcFlagSpring, MjcPhysicsTokens->mjcFlagDamper, - MjcPhysicsTokens->mjcFlagGravity, + // mjc:flag:gravity is deprecated, now exported as newton:gravityEnabled MjcPhysicsTokens->mjcFlagClampctrl, MjcPhysicsTokens->mjcFlagWarmstart, MjcPhysicsTokens->mjcFlagFilterparent, @@ -1083,6 +1103,10 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimDisableFlags) { ExpectAttributeEqual(stage, kPhysicsScenePrimPath.AppendProperty(flag), false); } + ExpectAttributeEqual( + stage, + kPhysicsScenePrimPath.AppendProperty(pxr::_tokens->newtonGravityEnabled), + false); } TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsScenePrimEnableFlags) { @@ -1472,8 +1496,12 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsCollisionAPI) { pxr::VtArray({0.1, 0.2})); ExpectAttributeEqual(stage, "/test/body/box.mjc:solimp", pxr::VtArray({0.3, 0.4, 0.5, 0.6, 0.7})); - ExpectAttributeEqual(stage, "/test/body/box.mjc:margin", 0.8); - ExpectAttributeEqual(stage, "/test/body/box.mjc:gap", 0.9); + // margin and gap are now exported as Newton attributes + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(stage, "/test/body/box.mjc:margin"); + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE(stage, "/test/body/box.mjc:gap"); + // newton:contactMargin = margin - gap = 0.8 - 0.9 = -0.1 + ExpectAttributeEqual(stage, "/test/body/box.newton:contactMargin", -0.1f); + ExpectAttributeEqual(stage, "/test/body/box.newton:contactGap", 0.9f); ExpectAttributeEqual(stage, "/test/body/box.mjc:shellinertia", true); } @@ -1508,8 +1536,11 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsMeshCollisionAPI) { MjcPhysicsTokens->convex); ExpectAttributeEqual(stage, "/test/body/tet_shell/Mesh.mjc:inertia", MjcPhysicsTokens->shell); - ExpectAttributeEqual(stage, "/test/body/tet_max_vert/Mesh.mjc:maxhullvert", - 12); + // mjc:maxhullvert deprecated, newton:maxHullVertices used instead + EXPECT_ATTRIBUTE_HAS_NO_AUTHORED_VALUE( + stage, "/test/body/tet_max_vert/Mesh.mjc:maxhullvert"); + ExpectAttributeEqual( + stage, "/test/body/tet_max_vert/Mesh.newton:maxHullVertices", 12); } TEST_F(MjcfSdfFileFormatPluginTest, TestMassAPIApplied) {