Skip to content

Commit e93a69e

Browse files
havesscopybara-github
authored andcommitted
Add MjcPhysicsEqualityConnectAPI to MjcPhysics for equality/connect parsing.
PiperOrigin-RevId: 867736241 Change-Id: I2d4e9abcb14280dcfbbf63ff5f888fb57350cfb6
1 parent 0041fdc commit e93a69e

File tree

9 files changed

+341
-56
lines changed

9 files changed

+341
-56
lines changed
Lines changed: 161 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,161 @@
1+
// Copyright 2025 DeepMind Technologies Limited
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef MJCPHYSICS_GENERATED_EQUALITYCONNECTAPI_H
16+
#define MJCPHYSICS_GENERATED_EQUALITYCONNECTAPI_H
17+
18+
/// \file mjcPhysics/equalityConnectAPI.h
19+
20+
#include <mujoco/experimental/usd/mjcPhysics/api.h>
21+
#include <pxr/base/gf/matrix4d.h>
22+
#include <pxr/base/gf/vec3d.h>
23+
#include <pxr/base/gf/vec3f.h>
24+
#include <pxr/base/tf/token.h>
25+
#include <pxr/base/tf/type.h>
26+
#include <pxr/base/vt/value.h>
27+
#include <pxr/pxr.h>
28+
#include <pxr/usd/usd/apiSchemaBase.h>
29+
#include <pxr/usd/usd/prim.h>
30+
#include <pxr/usd/usd/stage.h>
31+
32+
PXR_NAMESPACE_OPEN_SCOPE
33+
34+
class SdfAssetPath;
35+
36+
// -------------------------------------------------------------------------- //
37+
// MJCEQUALITYCONNECTAPI //
38+
// -------------------------------------------------------------------------- //
39+
40+
/// \class MjcPhysicsEqualityConnectAPI
41+
///
42+
/// API providing extension attributes to represent equality/connect
43+
/// constraints.
44+
///
45+
class MjcPhysicsEqualityConnectAPI : public UsdAPISchemaBase {
46+
public:
47+
/// Compile time constant representing what kind of schema this class is.
48+
///
49+
/// \sa UsdSchemaKind
50+
static const UsdSchemaKind schemaKind = UsdSchemaKind::SingleApplyAPI;
51+
52+
/// Construct a MjcPhysicsEqualityConnectAPI on UsdPrim \p prim .
53+
/// Equivalent to MjcPhysicsEqualityConnectAPI::Get(prim.GetStage(),
54+
/// prim.GetPath()) for a \em valid \p prim, but will not immediately throw an
55+
/// error for an invalid \p prim
56+
explicit MjcPhysicsEqualityConnectAPI(const UsdPrim& prim = UsdPrim())
57+
: UsdAPISchemaBase(prim) {}
58+
59+
/// Construct a MjcPhysicsEqualityConnectAPI on the prim held by \p schemaObj
60+
/// . Should be preferred over
61+
/// MjcPhysicsEqualityConnectAPI(schemaObj.GetPrim()), as it preserves
62+
/// SchemaBase state.
63+
explicit MjcPhysicsEqualityConnectAPI(const UsdSchemaBase& schemaObj)
64+
: UsdAPISchemaBase(schemaObj) {}
65+
66+
/// Destructor.
67+
MJCPHYSICS_API
68+
virtual ~MjcPhysicsEqualityConnectAPI();
69+
70+
/// Return a vector of names of all pre-declared attributes for this schema
71+
/// class and all its ancestor classes. Does not include attributes that
72+
/// may be authored by custom/extended methods of the schemas involved.
73+
MJCPHYSICS_API
74+
static const TfTokenVector& GetSchemaAttributeNames(
75+
bool includeInherited = true);
76+
77+
/// Return a MjcPhysicsEqualityConnectAPI holding the prim adhering to this
78+
/// schema at \p path on \p stage. If no prim exists at \p path on
79+
/// \p stage, or if the prim at that path does not adhere to this schema,
80+
/// return an invalid schema object. This is shorthand for the following:
81+
///
82+
/// \code
83+
/// MjcPhysicsEqualityConnectAPI(stage->GetPrimAtPath(path));
84+
/// \endcode
85+
///
86+
MJCPHYSICS_API
87+
static MjcPhysicsEqualityConnectAPI Get(const UsdStagePtr& stage,
88+
const SdfPath& path);
89+
90+
/// Returns true if this <b>single-apply</b> API schema can be applied to
91+
/// the given \p prim. If this schema can not be a applied to the prim,
92+
/// this returns false and, if provided, populates \p whyNot with the
93+
/// reason it can not be applied.
94+
///
95+
/// Note that if CanApply returns false, that does not necessarily imply
96+
/// that calling Apply will fail. Callers are expected to call CanApply
97+
/// before calling Apply if they want to ensure that it is valid to
98+
/// apply a schema.
99+
///
100+
/// \sa UsdPrim::GetAppliedSchemas()
101+
/// \sa UsdPrim::HasAPI()
102+
/// \sa UsdPrim::CanApplyAPI()
103+
/// \sa UsdPrim::ApplyAPI()
104+
/// \sa UsdPrim::RemoveAPI()
105+
///
106+
MJCPHYSICS_API
107+
static bool CanApply(const UsdPrim& prim, std::string* whyNot = nullptr);
108+
109+
/// Applies this <b>single-apply</b> API schema to the given \p prim.
110+
/// This information is stored by adding "MjcEqualityConnectAPI" to the
111+
/// token-valued, listOp metadata \em apiSchemas on the prim.
112+
///
113+
/// \return A valid MjcPhysicsEqualityConnectAPI object is returned upon
114+
/// success. An invalid (or empty) MjcPhysicsEqualityConnectAPI object is
115+
/// returned upon failure. See \ref UsdPrim::ApplyAPI() for conditions
116+
/// resulting in failure.
117+
///
118+
/// \sa UsdPrim::GetAppliedSchemas()
119+
/// \sa UsdPrim::HasAPI()
120+
/// \sa UsdPrim::CanApplyAPI()
121+
/// \sa UsdPrim::ApplyAPI()
122+
/// \sa UsdPrim::RemoveAPI()
123+
///
124+
MJCPHYSICS_API
125+
static MjcPhysicsEqualityConnectAPI Apply(const UsdPrim& prim);
126+
127+
protected:
128+
/// Returns the kind of schema this class belongs to.
129+
///
130+
/// \sa UsdSchemaKind
131+
MJCPHYSICS_API
132+
UsdSchemaKind _GetSchemaKind() const override;
133+
134+
private:
135+
// needs to invoke _GetStaticTfType.
136+
friend class UsdSchemaRegistry;
137+
MJCPHYSICS_API
138+
static const TfType& _GetStaticTfType();
139+
140+
static bool _IsTypedSchema();
141+
142+
// override SchemaBase virtuals.
143+
MJCPHYSICS_API
144+
const TfType& _GetTfType() const override;
145+
146+
public:
147+
// ===================================================================== //
148+
// Feel free to add custom code below this line, it will be preserved by
149+
// the code generator.
150+
//
151+
// Just remember to:
152+
// - Close the class declaration with };
153+
// - Close the namespace with PXR_NAMESPACE_CLOSE_SCOPE
154+
// - Close the include guard with #endif
155+
// ===================================================================== //
156+
// --(BEGIN CUSTOM CODE)--
157+
};
158+
159+
PXR_NAMESPACE_CLOSE_SCOPE
160+
161+
#endif

include/mujoco/experimental/usd/mjcPhysics/tokens.h

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -792,6 +792,10 @@ struct MjcPhysicsTokensType {
792792
///
793793
/// Schema identifier and family for MjcPhysicsEqualityAPI
794794
const TfToken MjcEqualityAPI;
795+
/// \brief "MjcEqualityConnectAPI"
796+
///
797+
/// Schema identifier and family for MjcPhysicsEqualityConnectAPI
798+
const TfToken MjcEqualityConnectAPI;
795799
/// \brief "MjcEqualityJointAPI"
796800
///
797801
/// Schema identifier and family for MjcPhysicsEqualityJointAPI

plugin/usd_decoder/usd_decoder.cc

Lines changed: 44 additions & 56 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
#include <mujoco/experimental/usd/mjcPhysics/actuator.h>
2727
#include <mujoco/experimental/usd/mjcPhysics/collisionAPI.h>
2828
#include <mujoco/experimental/usd/mjcPhysics/equalityAPI.h>
29+
#include <mujoco/experimental/usd/mjcPhysics/equalityConnectAPI.h>
2930
#include <mujoco/experimental/usd/mjcPhysics/equalityJointAPI.h>
3031
#include <mujoco/experimental/usd/mjcPhysics/equalityWeldAPI.h>
3132
#include <mujoco/experimental/usd/mjcPhysics/imageableAPI.h>
@@ -1829,6 +1830,42 @@ void ParseUsdPhysicsCollider(mjSpec* spec,
18291830
}
18301831
}
18311832

1833+
void ParseMjcEqualityAPISolverParams(
1834+
mjsEquality* eq, const pxr::MjcPhysicsEqualityAPI& equality_api,
1835+
const pxr::UsdPrim& prim) {
1836+
auto solref_attr = equality_api.GetSolRefAttr();
1837+
if (solref_attr.HasAuthoredValue()) {
1838+
pxr::VtDoubleArray solref;
1839+
solref_attr.Get(&solref);
1840+
if (solref.size() == mjNREF) {
1841+
for (int i = 0; i < mjNREF; ++i) {
1842+
eq->solref[i] = solref[i];
1843+
}
1844+
} else {
1845+
mju_warning(
1846+
"solref attribute for equality %s has incorrect size "
1847+
"%zu, expected %d.",
1848+
prim.GetPath().GetAsString().c_str(), solref.size(), mjNREF);
1849+
}
1850+
}
1851+
1852+
auto solimp_attr = equality_api.GetSolImpAttr();
1853+
if (solimp_attr.HasAuthoredValue()) {
1854+
pxr::VtDoubleArray solimp;
1855+
solimp_attr.Get(&solimp);
1856+
if (solimp.size() == mjNIMP) {
1857+
for (int i = 0; i < mjNIMP; ++i) {
1858+
eq->solimp[i] = solimp[i];
1859+
}
1860+
} else {
1861+
mju_warning(
1862+
"solimp attribute for equality %s has incorrect size "
1863+
"%zu, expected %d.",
1864+
prim.GetPath().GetAsString().c_str(), solimp.size(), mjNIMP);
1865+
}
1866+
}
1867+
}
1868+
18321869
void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
18331870
pxr::UsdGeomXformCache& xform_cache) {
18341871
if (prim.HasAPI<pxr::MjcPhysicsEqualityJointAPI>()) {
@@ -1861,28 +1898,7 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
18611898
eq_joint_api.GetCoef3Attr().Get(&eq->data[3]);
18621899
eq_joint_api.GetCoef4Attr().Get(&eq->data[4]);
18631900

1864-
// Parse solver parameters from MjcEqualityAPI.
1865-
auto solref_attr = equality_api.GetSolRefAttr();
1866-
if (solref_attr.HasAuthoredValue()) {
1867-
pxr::VtDoubleArray solref;
1868-
solref_attr.Get(&solref);
1869-
if (solref.size() == mjNREF) {
1870-
for (int i = 0; i < mjNREF; ++i) {
1871-
eq->solref[i] = solref[i];
1872-
}
1873-
}
1874-
}
1875-
1876-
auto solimp_attr = equality_api.GetSolImpAttr();
1877-
if (solimp_attr.HasAuthoredValue()) {
1878-
pxr::VtDoubleArray solimp;
1879-
solimp_attr.Get(&solimp);
1880-
if (solimp.size() == mjNIMP) {
1881-
for (int i = 0; i < mjNIMP; ++i) {
1882-
eq->solimp[i] = solimp[i];
1883-
}
1884-
}
1885-
}
1901+
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
18861902
} else if (prim.IsA<pxr::UsdPhysicsFixedJoint>()) {
18871903
// Handle fixed joints as weld constraints.
18881904
pxr::UsdPhysicsJoint joint(prim);
@@ -2013,41 +2029,13 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
20132029
eq->data[8] = relpose_quat.GetImaginary()[1];
20142030
eq->data[9] = relpose_quat.GetImaginary()[2];
20152031

2016-
if (prim.HasAPI<pxr::MjcPhysicsEqualityWeldAPI>()) {
2017-
// MjcPhysicsEqualityAPI is always automatically applied
2018-
// by MjcPhysicsEqualityWeldAPI.
2032+
if (prim.HasAPI<pxr::MjcPhysicsEqualityConnectAPI>()) {
2033+
eq->type = mjEQ_CONNECT;
20192034
pxr::MjcPhysicsEqualityAPI equality_api(prim);
2020-
auto solref_attr = equality_api.GetSolRefAttr();
2021-
if (solref_attr.HasAuthoredValue()) {
2022-
pxr::VtDoubleArray solref;
2023-
solref_attr.Get(&solref);
2024-
if (solref.size() == mjNREF) {
2025-
for (int i = 0; i < mjNREF; ++i) {
2026-
eq->solref[i] = solref[i];
2027-
}
2028-
} else {
2029-
mju_warning(
2030-
"solref attribute for weld equality %s has incorrect size "
2031-
"%zu, expected %d.",
2032-
prim.GetPath().GetAsString().c_str(), solref.size(), mjNREF);
2033-
}
2034-
}
2035-
2036-
auto solimp_attr = equality_api.GetSolImpAttr();
2037-
if (solimp_attr.HasAuthoredValue()) {
2038-
pxr::VtDoubleArray solimp;
2039-
solimp_attr.Get(&solimp);
2040-
if (solimp.size() == mjNIMP) {
2041-
for (int i = 0; i < mjNIMP; ++i) {
2042-
eq->solimp[i] = solimp[i];
2043-
}
2044-
} else {
2045-
mju_warning(
2046-
"solimp attribute for weld equality %s has incorrect size "
2047-
"%zu, expected %d.",
2048-
prim.GetPath().GetAsString().c_str(), solimp.size(), mjNIMP);
2049-
}
2050-
}
2035+
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
2036+
} else if (prim.HasAPI<pxr::MjcPhysicsEqualityWeldAPI>()) {
2037+
pxr::MjcPhysicsEqualityAPI equality_api(prim);
2038+
ParseMjcEqualityAPISolverParams(eq, equality_api, prim);
20512039

20522040
pxr::MjcPhysicsEqualityWeldAPI weld_api(prim);
20532041
auto torque_scale_attr = weld_api.GetTorqueScaleAttr();

src/experimental/usd/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,7 @@ target_sources(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PRIVATE
128128
mjcPhysics/actuator.cpp
129129
mjcPhysics/collisionAPI.cpp
130130
mjcPhysics/equalityAPI.cpp
131+
mjcPhysics/equalityConnectAPI.cpp
131132
mjcPhysics/equalityJointAPI.cpp
132133
mjcPhysics/equalityWeldAPI.cpp
133134
mjcPhysics/imageableAPI.cpp

0 commit comments

Comments
 (0)