From e93a69ea0143c4a4cdfddf8ea2686f62806b32e5 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Mon, 9 Feb 2026 13:01:29 -0800 Subject: [PATCH] Add MjcPhysicsEqualityConnectAPI to MjcPhysics for equality/connect parsing. PiperOrigin-RevId: 867736241 Change-Id: I2d4e9abcb14280dcfbbf63ff5f888fb57350cfb6 --- .../usd/mjcPhysics/equalityConnectAPI.h | 161 ++++++++++++++++++ .../experimental/usd/mjcPhysics/tokens.h | 4 + plugin/usd_decoder/usd_decoder.cc | 100 +++++------ src/experimental/usd/CMakeLists.txt | 1 + .../usd/mjcPhysics/equalityConnectAPI.cpp | 102 +++++++++++ .../usd/mjcPhysics/generatedSchema.usda | 7 + src/experimental/usd/mjcPhysics/plugInfo.json | 10 ++ src/experimental/usd/mjcPhysics/schema.usda | 10 ++ src/experimental/usd/mjcPhysics/tokens.cpp | 2 + 9 files changed, 341 insertions(+), 56 deletions(-) create mode 100644 include/mujoco/experimental/usd/mjcPhysics/equalityConnectAPI.h create mode 100644 src/experimental/usd/mjcPhysics/equalityConnectAPI.cpp diff --git a/include/mujoco/experimental/usd/mjcPhysics/equalityConnectAPI.h b/include/mujoco/experimental/usd/mjcPhysics/equalityConnectAPI.h new file mode 100644 index 00000000..104d11e1 --- /dev/null +++ b/include/mujoco/experimental/usd/mjcPhysics/equalityConnectAPI.h @@ -0,0 +1,161 @@ +// 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. + +#ifndef MJCPHYSICS_GENERATED_EQUALITYCONNECTAPI_H +#define MJCPHYSICS_GENERATED_EQUALITYCONNECTAPI_H + +/// \file mjcPhysics/equalityConnectAPI.h + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +class SdfAssetPath; + +// -------------------------------------------------------------------------- // +// MJCEQUALITYCONNECTAPI // +// -------------------------------------------------------------------------- // + +/// \class MjcPhysicsEqualityConnectAPI +/// +/// API providing extension attributes to represent equality/connect +/// constraints. +/// +class MjcPhysicsEqualityConnectAPI : public UsdAPISchemaBase { + public: + /// Compile time constant representing what kind of schema this class is. + /// + /// \sa UsdSchemaKind + static const UsdSchemaKind schemaKind = UsdSchemaKind::SingleApplyAPI; + + /// Construct a MjcPhysicsEqualityConnectAPI on UsdPrim \p prim . + /// Equivalent to MjcPhysicsEqualityConnectAPI::Get(prim.GetStage(), + /// prim.GetPath()) for a \em valid \p prim, but will not immediately throw an + /// error for an invalid \p prim + explicit MjcPhysicsEqualityConnectAPI(const UsdPrim& prim = UsdPrim()) + : UsdAPISchemaBase(prim) {} + + /// Construct a MjcPhysicsEqualityConnectAPI on the prim held by \p schemaObj + /// . Should be preferred over + /// MjcPhysicsEqualityConnectAPI(schemaObj.GetPrim()), as it preserves + /// SchemaBase state. + explicit MjcPhysicsEqualityConnectAPI(const UsdSchemaBase& schemaObj) + : UsdAPISchemaBase(schemaObj) {} + + /// Destructor. + MJCPHYSICS_API + virtual ~MjcPhysicsEqualityConnectAPI(); + + /// Return a vector of names of all pre-declared attributes for this schema + /// class and all its ancestor classes. Does not include attributes that + /// may be authored by custom/extended methods of the schemas involved. + MJCPHYSICS_API + static const TfTokenVector& GetSchemaAttributeNames( + bool includeInherited = true); + + /// Return a MjcPhysicsEqualityConnectAPI holding the prim adhering to this + /// schema at \p path on \p stage. If no prim exists at \p path on + /// \p stage, or if the prim at that path does not adhere to this schema, + /// return an invalid schema object. This is shorthand for the following: + /// + /// \code + /// MjcPhysicsEqualityConnectAPI(stage->GetPrimAtPath(path)); + /// \endcode + /// + MJCPHYSICS_API + static MjcPhysicsEqualityConnectAPI Get(const UsdStagePtr& stage, + const SdfPath& path); + + /// Returns true if this single-apply API schema can be applied to + /// the given \p prim. If this schema can not be a applied to the prim, + /// this returns false and, if provided, populates \p whyNot with the + /// reason it can not be applied. + /// + /// Note that if CanApply returns false, that does not necessarily imply + /// that calling Apply will fail. Callers are expected to call CanApply + /// before calling Apply if they want to ensure that it is valid to + /// apply a schema. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static bool CanApply(const UsdPrim& prim, std::string* whyNot = nullptr); + + /// Applies this single-apply API schema to the given \p prim. + /// This information is stored by adding "MjcEqualityConnectAPI" to the + /// token-valued, listOp metadata \em apiSchemas on the prim. + /// + /// \return A valid MjcPhysicsEqualityConnectAPI object is returned upon + /// success. An invalid (or empty) MjcPhysicsEqualityConnectAPI object is + /// returned upon failure. See \ref UsdPrim::ApplyAPI() for conditions + /// resulting in failure. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static MjcPhysicsEqualityConnectAPI Apply(const UsdPrim& prim); + + protected: + /// Returns the kind of schema this class belongs to. + /// + /// \sa UsdSchemaKind + MJCPHYSICS_API + UsdSchemaKind _GetSchemaKind() const override; + + private: + // needs to invoke _GetStaticTfType. + friend class UsdSchemaRegistry; + MJCPHYSICS_API + static const TfType& _GetStaticTfType(); + + static bool _IsTypedSchema(); + + // override SchemaBase virtuals. + MJCPHYSICS_API + const TfType& _GetTfType() const override; + + public: + // ===================================================================== // + // Feel free to add custom code below this line, it will be preserved by + // the code generator. + // + // Just remember to: + // - Close the class declaration with }; + // - Close the namespace with PXR_NAMESPACE_CLOSE_SCOPE + // - Close the include guard with #endif + // ===================================================================== // + // --(BEGIN CUSTOM CODE)-- +}; + +PXR_NAMESPACE_CLOSE_SCOPE + +#endif diff --git a/include/mujoco/experimental/usd/mjcPhysics/tokens.h b/include/mujoco/experimental/usd/mjcPhysics/tokens.h index 37dff9c4..53796b06 100644 --- a/include/mujoco/experimental/usd/mjcPhysics/tokens.h +++ b/include/mujoco/experimental/usd/mjcPhysics/tokens.h @@ -792,6 +792,10 @@ struct MjcPhysicsTokensType { /// /// Schema identifier and family for MjcPhysicsEqualityAPI const TfToken MjcEqualityAPI; + /// \brief "MjcEqualityConnectAPI" + /// + /// Schema identifier and family for MjcPhysicsEqualityConnectAPI + const TfToken MjcEqualityConnectAPI; /// \brief "MjcEqualityJointAPI" /// /// Schema identifier and family for MjcPhysicsEqualityJointAPI diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index a35971d2..ebf2bb5d 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -1829,6 +1830,42 @@ void ParseUsdPhysicsCollider(mjSpec* spec, } } +void ParseMjcEqualityAPISolverParams( + mjsEquality* eq, const pxr::MjcPhysicsEqualityAPI& equality_api, + const pxr::UsdPrim& prim) { + auto solref_attr = equality_api.GetSolRefAttr(); + 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]; + } + } else { + mju_warning( + "solref attribute for equality %s has incorrect size " + "%zu, expected %d.", + prim.GetPath().GetAsString().c_str(), solref.size(), mjNREF); + } + } + + auto solimp_attr = equality_api.GetSolImpAttr(); + 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 { + mju_warning( + "solimp attribute for equality %s has incorrect size " + "%zu, expected %d.", + prim.GetPath().GetAsString().c_str(), solimp.size(), mjNIMP); + } + } +} + void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, pxr::UsdGeomXformCache& xform_cache) { if (prim.HasAPI()) { @@ -1861,28 +1898,7 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, eq_joint_api.GetCoef3Attr().Get(&eq->data[3]); eq_joint_api.GetCoef4Attr().Get(&eq->data[4]); - // Parse solver parameters from MjcEqualityAPI. - auto solref_attr = equality_api.GetSolRefAttr(); - 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 = equality_api.GetSolImpAttr(); - 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]; - } - } - } + ParseMjcEqualityAPISolverParams(eq, equality_api, prim); } else if (prim.IsA()) { // Handle fixed joints as weld constraints. pxr::UsdPhysicsJoint joint(prim); @@ -2013,41 +2029,13 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, eq->data[8] = relpose_quat.GetImaginary()[1]; eq->data[9] = relpose_quat.GetImaginary()[2]; - if (prim.HasAPI()) { - // MjcPhysicsEqualityAPI is always automatically applied - // by MjcPhysicsEqualityWeldAPI. + if (prim.HasAPI()) { + eq->type = mjEQ_CONNECT; pxr::MjcPhysicsEqualityAPI equality_api(prim); - auto solref_attr = equality_api.GetSolRefAttr(); - 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]; - } - } else { - mju_warning( - "solref attribute for weld equality %s has incorrect size " - "%zu, expected %d.", - prim.GetPath().GetAsString().c_str(), solref.size(), mjNREF); - } - } - - auto solimp_attr = equality_api.GetSolImpAttr(); - 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 { - mju_warning( - "solimp attribute for weld equality %s has incorrect size " - "%zu, expected %d.", - prim.GetPath().GetAsString().c_str(), solimp.size(), mjNIMP); - } - } + ParseMjcEqualityAPISolverParams(eq, equality_api, prim); + } else if (prim.HasAPI()) { + pxr::MjcPhysicsEqualityAPI equality_api(prim); + ParseMjcEqualityAPISolverParams(eq, equality_api, prim); pxr::MjcPhysicsEqualityWeldAPI weld_api(prim); auto torque_scale_attr = weld_api.GetTorqueScaleAttr(); diff --git a/src/experimental/usd/CMakeLists.txt b/src/experimental/usd/CMakeLists.txt index 3a5b79ba..c2763ef3 100644 --- a/src/experimental/usd/CMakeLists.txt +++ b/src/experimental/usd/CMakeLists.txt @@ -128,6 +128,7 @@ target_sources(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PRIVATE mjcPhysics/actuator.cpp mjcPhysics/collisionAPI.cpp mjcPhysics/equalityAPI.cpp + mjcPhysics/equalityConnectAPI.cpp mjcPhysics/equalityJointAPI.cpp mjcPhysics/equalityWeldAPI.cpp mjcPhysics/imageableAPI.cpp diff --git a/src/experimental/usd/mjcPhysics/equalityConnectAPI.cpp b/src/experimental/usd/mjcPhysics/equalityConnectAPI.cpp new file mode 100644 index 00000000..17442fc3 --- /dev/null +++ b/src/experimental/usd/mjcPhysics/equalityConnectAPI.cpp @@ -0,0 +1,102 @@ +// 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 +#include + +PXR_NAMESPACE_OPEN_SCOPE + +// Register the schema with the TfType system. +TF_REGISTRY_FUNCTION(TfType) { + TfType::Define >(); +} + +/* virtual */ +MjcPhysicsEqualityConnectAPI::~MjcPhysicsEqualityConnectAPI() {} + +/* static */ +MjcPhysicsEqualityConnectAPI MjcPhysicsEqualityConnectAPI::Get( + const UsdStagePtr& stage, const SdfPath& path) { + if (!stage) { + TF_CODING_ERROR("Invalid stage"); + return MjcPhysicsEqualityConnectAPI(); + } + return MjcPhysicsEqualityConnectAPI(stage->GetPrimAtPath(path)); +} + +/* virtual */ +UsdSchemaKind MjcPhysicsEqualityConnectAPI::_GetSchemaKind() const { + return MjcPhysicsEqualityConnectAPI::schemaKind; +} + +/* static */ +bool MjcPhysicsEqualityConnectAPI::CanApply(const UsdPrim& prim, + std::string* whyNot) { + return prim.CanApplyAPI(whyNot); +} + +/* static */ +MjcPhysicsEqualityConnectAPI MjcPhysicsEqualityConnectAPI::Apply( + const UsdPrim& prim) { + if (prim.ApplyAPI()) { + return MjcPhysicsEqualityConnectAPI(prim); + } + return MjcPhysicsEqualityConnectAPI(); +} + +/* static */ +const TfType& MjcPhysicsEqualityConnectAPI::_GetStaticTfType() { + static TfType tfType = TfType::Find(); + return tfType; +} + +/* static */ +bool MjcPhysicsEqualityConnectAPI::_IsTypedSchema() { + static bool isTyped = _GetStaticTfType().IsA(); + return isTyped; +} + +/* virtual */ +const TfType& MjcPhysicsEqualityConnectAPI::_GetTfType() const { + return _GetStaticTfType(); +} + +/*static*/ +const TfTokenVector& MjcPhysicsEqualityConnectAPI::GetSchemaAttributeNames( + bool includeInherited) { + static TfTokenVector localNames; + static TfTokenVector allNames = + UsdAPISchemaBase::GetSchemaAttributeNames(true); + + if (includeInherited) + return allNames; + else + return localNames; +} + +PXR_NAMESPACE_CLOSE_SCOPE + +// ===================================================================== // +// Feel free to add custom code below this line. It will be preserved by +// the code generator. +// +// Just remember to wrap code in the appropriate delimiters: +// 'PXR_NAMESPACE_OPEN_SCOPE', 'PXR_NAMESPACE_CLOSE_SCOPE'. +// ===================================================================== // +// --(BEGIN CUSTOM CODE)-- diff --git a/src/experimental/usd/mjcPhysics/generatedSchema.usda b/src/experimental/usd/mjcPhysics/generatedSchema.usda index ff607a84..633ec10d 100644 --- a/src/experimental/usd/mjcPhysics/generatedSchema.usda +++ b/src/experimental/usd/mjcPhysics/generatedSchema.usda @@ -567,6 +567,13 @@ class "MjcEqualityAPI" ( ) } +class "MjcEqualityConnectAPI" ( + apiSchemas = ["MjcEqualityAPI"] + doc = "API providing extension attributes to represent equality/connect constraints." +) +{ +} + class "MjcEqualityWeldAPI" ( apiSchemas = ["MjcEqualityAPI"] doc = "API providing extension attributes to represent equality/weld constraints." diff --git a/src/experimental/usd/mjcPhysics/plugInfo.json b/src/experimental/usd/mjcPhysics/plugInfo.json index f00dd78c..bac90147 100644 --- a/src/experimental/usd/mjcPhysics/plugInfo.json +++ b/src/experimental/usd/mjcPhysics/plugInfo.json @@ -33,6 +33,16 @@ ], "schemaKind": "singleApplyAPI" }, + "MjcPhysicsEqualityConnectAPI": { + "alias": { + "UsdSchemaBase": "MjcEqualityConnectAPI" + }, + "autoGenerated": true, + "bases": [ + "UsdAPISchemaBase" + ], + "schemaKind": "singleApplyAPI" + }, "MjcPhysicsEqualityJointAPI": { "alias": { "UsdSchemaBase": "MjcEqualityJointAPI" diff --git a/src/experimental/usd/mjcPhysics/schema.usda b/src/experimental/usd/mjcPhysics/schema.usda index 5738b5a0..62c1d993 100644 --- a/src/experimental/usd/mjcPhysics/schema.usda +++ b/src/experimental/usd/mjcPhysics/schema.usda @@ -1087,6 +1087,16 @@ class "MjcEqualityAPI" ( ) } +class "MjcEqualityConnectAPI" ( + customData = { + string className = "EqualityConnectAPI" + } + doc = """API providing extension attributes to represent equality/connect constraints.""" + + prepend apiSchemas = ["MjcEqualityAPI"] + inherits = +){} + class "MjcEqualityWeldAPI" ( customData = { string className = "EqualityWeldAPI" diff --git a/src/experimental/usd/mjcPhysics/tokens.cpp b/src/experimental/usd/mjcPhysics/tokens.cpp index f87aaa61..c278e715 100644 --- a/src/experimental/usd/mjcPhysics/tokens.cpp +++ b/src/experimental/usd/mjcPhysics/tokens.cpp @@ -199,6 +199,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() MjcActuator("MjcActuator", TfToken::Immortal), MjcCollisionAPI("MjcCollisionAPI", TfToken::Immortal), MjcEqualityAPI("MjcEqualityAPI", TfToken::Immortal), + MjcEqualityConnectAPI("MjcEqualityConnectAPI", TfToken::Immortal), MjcEqualityJointAPI("MjcEqualityJointAPI", TfToken::Immortal), MjcEqualityWeldAPI("MjcEqualityWeldAPI", TfToken::Immortal), MjcImageableAPI("MjcImageableAPI", TfToken::Immortal), @@ -384,6 +385,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() MjcActuator, MjcCollisionAPI, MjcEqualityAPI, + MjcEqualityConnectAPI, MjcEqualityJointAPI, MjcEqualityWeldAPI, MjcImageableAPI,