Add MjcPhysicsEqualityConnectAPI to MjcPhysics for equality/connect parsing.

PiperOrigin-RevId: 867736241
Change-Id: I2d4e9abcb14280dcfbbf63ff5f888fb57350cfb6
This commit is contained in:
Sam Haves
2026-02-09 13:01:29 -08:00
committed by Copybara-Service
parent 0041fdcbb0
commit e93a69ea01
9 changed files with 341 additions and 56 deletions
@@ -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 <mujoco/experimental/usd/mjcPhysics/api.h>
#include <pxr/base/gf/matrix4d.h>
#include <pxr/base/gf/vec3d.h>
#include <pxr/base/gf/vec3f.h>
#include <pxr/base/tf/token.h>
#include <pxr/base/tf/type.h>
#include <pxr/base/vt/value.h>
#include <pxr/pxr.h>
#include <pxr/usd/usd/apiSchemaBase.h>
#include <pxr/usd/usd/prim.h>
#include <pxr/usd/usd/stage.h>
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 <b>single-apply</b> 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 <b>single-apply</b> 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
@@ -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
+44 -56
View File
@@ -26,6 +26,7 @@
#include <mujoco/experimental/usd/mjcPhysics/actuator.h>
#include <mujoco/experimental/usd/mjcPhysics/collisionAPI.h>
#include <mujoco/experimental/usd/mjcPhysics/equalityAPI.h>
#include <mujoco/experimental/usd/mjcPhysics/equalityConnectAPI.h>
#include <mujoco/experimental/usd/mjcPhysics/equalityJointAPI.h>
#include <mujoco/experimental/usd/mjcPhysics/equalityWeldAPI.h>
#include <mujoco/experimental/usd/mjcPhysics/imageableAPI.h>
@@ -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<pxr::MjcPhysicsEqualityJointAPI>()) {
@@ -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<pxr::UsdPhysicsFixedJoint>()) {
// 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<pxr::MjcPhysicsEqualityWeldAPI>()) {
// MjcPhysicsEqualityAPI is always automatically applied
// by MjcPhysicsEqualityWeldAPI.
if (prim.HasAPI<pxr::MjcPhysicsEqualityConnectAPI>()) {
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::MjcPhysicsEqualityWeldAPI>()) {
pxr::MjcPhysicsEqualityAPI equality_api(prim);
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
pxr::MjcPhysicsEqualityWeldAPI weld_api(prim);
auto torque_scale_attr = weld_api.GetTorqueScaleAttr();
+1
View File
@@ -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
@@ -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 <mujoco/experimental/usd/mjcPhysics/equalityConnectAPI.h>
#include <pxr/usd/sdf/assetPath.h>
#include <pxr/usd/sdf/types.h>
#include <pxr/usd/usd/schemaRegistry.h>
#include <pxr/usd/usd/typed.h>
PXR_NAMESPACE_OPEN_SCOPE
// Register the schema with the TfType system.
TF_REGISTRY_FUNCTION(TfType) {
TfType::Define<MjcPhysicsEqualityConnectAPI,
TfType::Bases<UsdAPISchemaBase> >();
}
/* 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<MjcPhysicsEqualityConnectAPI>(whyNot);
}
/* static */
MjcPhysicsEqualityConnectAPI MjcPhysicsEqualityConnectAPI::Apply(
const UsdPrim& prim) {
if (prim.ApplyAPI<MjcPhysicsEqualityConnectAPI>()) {
return MjcPhysicsEqualityConnectAPI(prim);
}
return MjcPhysicsEqualityConnectAPI();
}
/* static */
const TfType& MjcPhysicsEqualityConnectAPI::_GetStaticTfType() {
static TfType tfType = TfType::Find<MjcPhysicsEqualityConnectAPI>();
return tfType;
}
/* static */
bool MjcPhysicsEqualityConnectAPI::_IsTypedSchema() {
static bool isTyped = _GetStaticTfType().IsA<UsdTyped>();
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)--
@@ -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."
@@ -33,6 +33,16 @@
],
"schemaKind": "singleApplyAPI"
},
"MjcPhysicsEqualityConnectAPI": {
"alias": {
"UsdSchemaBase": "MjcEqualityConnectAPI"
},
"autoGenerated": true,
"bases": [
"UsdAPISchemaBase"
],
"schemaKind": "singleApplyAPI"
},
"MjcPhysicsEqualityJointAPI": {
"alias": {
"UsdSchemaBase": "MjcEqualityJointAPI"
@@ -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 = </APISchemaBase>
){}
class "MjcEqualityWeldAPI" (
customData = {
string className = "EqualityWeldAPI"
@@ -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,